From 4f2934957f53bd8d64de89172b3afb71ca4e9549 Mon Sep 17 00:00:00 2001 From: Gus Grubba Date: Mon, 15 Jul 2019 17:41:11 -0400 Subject: [PATCH] Initial work Add camera trigger Add video start/stop/toggle recording Add gimbal pitch and yaw --- src/Joystick/Joystick.cc | 133 ++++---- src/Joystick/Joystick.h | 80 ++--- src/VehicleSetup/JoystickConfig.qml | 84 +++-- src/VehicleSetup/JoystickConfigController.cc | 303 ++++++++----------- src/VehicleSetup/JoystickConfigController.h | 184 ++++++----- 5 files changed, 395 insertions(+), 389 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index c8d404ec4..f521d2109 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -19,7 +19,7 @@ #include -QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog") +QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog") QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog") const char* Joystick::_settingsGroup = "Joysticks"; @@ -48,56 +48,44 @@ const char* Joystick::_buttonActionNextStream = QT_TR_NOOP("Next Video S const char* Joystick::_buttonActionPreviousStream = QT_TR_NOOP("Previous Video Stream"); const char* Joystick::_buttonActionNextCamera = QT_TR_NOOP("Next Camera"); const char* Joystick::_buttonActionPreviousCamera = QT_TR_NOOP("Previous Camera"); +const char* Joystick::_buttonActionTriggerCamera = QT_TR_NOOP("Trigger Camera"); +const char* Joystick::_buttonActionStartVideoRecord = QT_TR_NOOP("Start Recording Video"); +const char* Joystick::_buttonActionStopVideoRecord = QT_TR_NOOP("Stop Recording Video"); +const char* Joystick::_buttonActionToggleVideoRecord = QT_TR_NOOP("Toggle Recording Video"); const char* Joystick::_rgFunctionSettingsKey[Joystick::maxFunction] = { "RollAxis", "PitchAxis", "YawAxis", - "ThrottleAxis" + "ThrottleAxis", + "GimbalPitchAxis", + "GimbalYawAxis" }; int Joystick::_transmitterMode = 2; Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatCount, MultiVehicleManager* multiVehicleManager) - : _exitThread(false) - , _name(name) + : _name(name) , _axisCount(axisCount) , _buttonCount(buttonCount) , _hatCount(hatCount) - , _hatButtonCount(4*hatCount) + , _hatButtonCount(4 * hatCount) , _totalButtonCount(_buttonCount+_hatButtonCount) - , _calibrationMode(false) - , _rgAxisValues(nullptr) - , _rgCalibration(nullptr) - , _rgButtonValues(nullptr) - , _lastButtonBits(0) - , _throttleMode(ThrottleModeDownZero) - , _negativeThrust(false) - , _exponential(0) - , _accumulator(false) - , _deadband(false) - , _circleCorrection(true) - , _frequency(25.0f) - , _activeVehicle(nullptr) - , _pollingStartedForCalibration(false) , _multiVehicleManager(multiVehicleManager) { - - _rgAxisValues = new int[_axisCount]; - _rgCalibration = new Calibration_t[_axisCount]; + _rgAxisValues = new int[_axisCount]; + _rgCalibration = new Calibration_t[_axisCount]; _rgButtonValues = new bool[_totalButtonCount]; - for (int i=0; i<_axisCount; i++) { + for (int i = 0; i < _axisCount; i++) { _rgAxisValues[i] = 0; } - for (int i=0; i<_totalButtonCount; i++) { + for (int i = 0; i < _totalButtonCount; i++) { _rgButtonValues[i] = false; } _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle()); - _loadSettings(); - connect(_multiVehicleManager, &MultiVehicleManager::activeVehicleChanged, this, &Joystick::_activeVehicleChanged); } @@ -135,13 +123,16 @@ void Joystick::_setDefaultCalibration(void) { _rgFunctionAxis[yawFunction] = 0; _rgFunctionAxis[throttleFunction] = 1; - _exponential = 0; - _accumulator = false; - _deadband = false; + _rgFunctionAxis[gimbalPitchFunction]= 4; + _rgFunctionAxis[gimbalYawFunction] = 5; + + _exponential = 0; + _accumulator = false; + _deadband = false; + _frequency = 25.0f; + _throttleMode = ThrottleModeDownZero; + _calibrated = true; _circleCorrection = false; - _frequency = 25.0f; - _throttleMode = ThrottleModeDownZero; - _calibrated = true; _saveSettings(); } @@ -182,7 +173,7 @@ void Joystick::_activeVehicleChanged(Vehicle* activeVehicle) } } -void Joystick::_loadSettings(void) +void Joystick::_loadSettings() { QSettings settings; @@ -239,14 +230,13 @@ void Joystick::_loadSettings(void) qCDebug(JoystickLog) << "_loadSettings axis:min:max:trim:reversed:deadband:badsettings" << axis << calibration->min << calibration->max << calibration->center << calibration->reversed << calibration->deadband << badSettings; } - for (int function=0; function= _axisCount); + if(functionAxis < _axisCount) { + _rgFunctionAxis[function] = functionAxis; + } qCDebug(JoystickLog) << "_loadSettings function:axis:badsettings" << function << functionAxis << badSettings; } @@ -265,7 +255,7 @@ void Joystick::_loadSettings(void) } } -void Joystick::_saveSettings(void) +void Joystick::_saveSettings() { QSettings settings; @@ -429,7 +419,7 @@ void Joystick::run(void) _update(); // Update axes - for (int axisIndex=0; axisIndex<_axisCount; axisIndex++) { + for (int axisIndex = 0; axisIndex < _axisCount; axisIndex++) { int newAxisValue = _getAxis(axisIndex); // Calibration code requires signal to be emitted even if value hasn't changed _rgAxisValues[axisIndex] = newAxisValue; @@ -437,7 +427,7 @@ void Joystick::run(void) } // Update buttons - for (int buttonIndex=0; buttonIndex<_buttonCount; buttonIndex++) { + for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) { bool newButtonValue = _getButton(buttonIndex); if (newButtonValue != _rgButtonValues[buttonIndex]) { _rgButtonValues[buttonIndex] = newButtonValue; @@ -447,8 +437,8 @@ void Joystick::run(void) // Update hat - append hat buttons to the end of the normal button list int numHatButtons = 4; - for (int hatIndex=0; hatIndex<_hatCount; hatIndex++) { - for (int hatButtonIndex=0; hatButtonIndex(-1.f), std::min(throttle_accu, static_cast(1.f))); throttle = throttle_accu; } if ( _circleCorrection ) { - float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); - float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); - float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); - float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); + float roll_limited = std::max(static_cast(-M_PI_4), std::min(roll, static_cast(M_PI_4))); + float pitch_limited = std::max(static_cast(-M_PI_4), std::min(pitch, static_cast(M_PI_4))); + float yaw_limited = std::max(static_cast(-M_PI_4), std::min(yaw, static_cast(M_PI_4))); + float throttle_limited = std::max(static_cast(-M_PI_4), std::min(throttle, static_cast(M_PI_4))); // Map from unit circle to linear range and limit - roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); - pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f)); - yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f)); + roll = std::max(-1.0f, std::min(tanf(asinf(roll_limited)), 1.0f)); + pitch = std::max(-1.0f, std::min(tanf(asinf(pitch_limited)), 1.0f)); + yaw = std::max(-1.0f, std::min(tanf(asinf(yaw_limited)), 1.0f)); throttle = std::max(-1.0f, std::min(tanf(asinf(throttle_limited)), 1.0f)); } @@ -500,9 +494,9 @@ void Joystick::run(void) //_exponential is set by a slider in joystickConfig.qml // Calculate new RPY with exponential applied - roll = -_exponential*powf(roll,3) + (1+_exponential)*roll; + roll = -_exponential*powf(roll, 3) + (1+_exponential)*roll; pitch = -_exponential*powf(pitch,3) + (1+_exponential)*pitch; - yaw = -_exponential*powf(yaw,3) + (1+_exponential)*yaw; + yaw = -_exponential*powf(yaw, 3) + (1+_exponential)*yaw; } // Adjust throttle to 0:1 range @@ -516,10 +510,10 @@ void Joystick::run(void) // Set up button pressed information - quint16 newButtonBits = 0; // New set of button which are down - quint16 buttonPressedBits = 0; // Buttons pressed for manualControl signal + quint16 newButtonBits = 0; // New set of button which are down + quint16 buttonPressedBits = 0; // Buttons pressed for manualControl signal - for (int buttonIndex=0; buttonIndex<_totalButtonCount; buttonIndex++) { + for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) { quint16 buttonBit = 1 << buttonIndex; if (!_rgButtonValues[buttonIndex]) { @@ -529,7 +523,6 @@ void Joystick::run(void) if (_lastButtonBits & buttonBit) { // Button was up last time through, but is now down which indicates a button press qCDebug(JoystickLog) << "button triggered" << buttonIndex; - QString buttonAction =_rgButtonActions[buttonIndex]; if (!buttonAction.isEmpty()) { _buttonAction(buttonAction); @@ -542,15 +535,13 @@ void Joystick::run(void) } _lastButtonBits = newButtonBits; - qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle" << name() << roll << -pitch << yaw << throttle; - // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub. - emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode()); + emit manualControl(roll, -pitch, yaw, throttle, gimbalPitch, gimbalYaw, buttonPressedBits, _activeVehicle->joystickMode()); } // Sleep. Update rate of joystick is by default 25 Hz - int mswait = (int)(1000.0f / _frequency); + unsigned long mswait = static_cast(1000.0f / _frequency); QGC::SLEEP::msleep(mswait); } @@ -644,17 +635,15 @@ void Joystick::setFunctionAxis(AxisFunction_t function, int axis) _calibrated = true; _rgFunctionAxis[function] = axis; - _saveSettings(); emit calibratedChanged(_calibrated); } int Joystick::getFunctionAxis(AxisFunction_t function) { - if (function < 0 || function >= maxFunction) { + if (static_cast(function) < 0 || function >= maxFunction) { qCWarning(JoystickLog) << "Invalid function" << function; } - return _rgFunctionAxis[function]; } @@ -669,6 +658,10 @@ QStringList Joystick::actions(void) list << _buttonActionZoomIn << _buttonActionZoomOut; list << _buttonActionNextStream << _buttonActionPreviousStream; list << _buttonActionNextCamera << _buttonActionPreviousCamera; + list << _buttonActionTriggerCamera; + list << _buttonActionStartVideoRecord; + list << _buttonActionStopVideoRecord; + list << _buttonActionToggleVideoRecord; return list; } @@ -846,6 +839,14 @@ void Joystick::_buttonAction(const QString& action) emit stepStream(action == _buttonActionNextStream ? 1 : -1); } else if(action == _buttonActionNextCamera || action == _buttonActionPreviousCamera) { emit stepCamera(action == _buttonActionNextCamera ? 1 : -1); + } else if(action == _buttonActionTriggerCamera) { + emit triggerCamera(); + } else if(action == _buttonActionStartVideoRecord) { + emit startVideoRecord(); + } else if(action == _buttonActionStopVideoRecord) { + emit stopVideoRecord(); + } else if(action == _buttonActionToggleVideoRecord) { + emit toggleVideoRecord(); } else { qCDebug(JoystickLog) << "_buttonAction unknown action:" << action; } diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index c65dd6ab4..0dbb0ad03 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -49,6 +49,8 @@ public: pitchFunction, yawFunction, throttleFunction, + gimbalPitchFunction, + gimbalYawFunction, maxFunction } AxisFunction_t; @@ -155,12 +157,14 @@ signals: void circleCorrectionChanged(bool circleCorrection); /// Signal containing new joystick information - /// @param roll Range is -1:1, negative meaning roll left, positive meaning roll right - /// @param pitch Range i -1:1, negative meaning pitch down, positive meaning pitch up - /// @param yaw Range is -1:1, negative meaning yaw left, positive meaning yaw right - /// @param throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle + /// @param roll Range is -1:1, negative meaning roll left, positive meaning roll right + /// @param pitch Range i -1:1, negative meaning pitch down, positive meaning pitch up + /// @param yaw Range is -1:1, negative meaning yaw left, positive meaning yaw right + /// @param throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle + /// @param gimbalPitch Range is -1:1 + /// @param gimbalYaw Range is -1:1 /// @param mode See Vehicle::JoystickMode_t enum - void manualControl(float roll, float pitch, float yaw, float throttle, quint16 buttons, int joystickMmode); + void manualControl (float roll, float pitch, float yaw, float throttle, float gimbalPitch, float gimbalYaw, quint16 buttons, int joystickMmode); void buttonActionTriggered(int action); @@ -168,20 +172,24 @@ signals: void stepZoom (int direction); void stepCamera (int direction); void stepStream (int direction); + void triggerCamera (); + void startVideoRecord (); + void stopVideoRecord (); + void toggleVideoRecord (); protected: - void _setDefaultCalibration(void); - void _saveSettings(void); - void _loadSettings(void); - float _adjustRange(int value, Calibration_t calibration, bool withDeadbands); - void _buttonAction(const QString& action); - bool _validAxis(int axis); - bool _validButton(int button); + void _setDefaultCalibration (); + void _saveSettings (); + void _loadSettings (); + float _adjustRange (int value, Calibration_t calibration, bool withDeadbands); + void _buttonAction (const QString& action); + bool _validAxis (int axis); + bool _validButton (int button); private: - virtual bool _open() = 0; - virtual void _close() = 0; - virtual bool _update() = 0; + virtual bool _open() = 0; + virtual void _close() = 0; + virtual bool _update() = 0; virtual bool _getButton(int i) = 0; virtual int _getAxis(int i) = 0; @@ -196,7 +204,23 @@ private: protected: - bool _exitThread; ///< true: signal thread to exit + bool _exitThread = false; ///< true: signal thread to exit + bool _calibrationMode = false; + int* _rgAxisValues = nullptr; + Calibration_t* _rgCalibration = nullptr; + bool* _rgButtonValues = nullptr; + quint16 _lastButtonBits = 0; + ThrottleMode_t _throttleMode = ThrottleModeDownZero; + bool _negativeThrust = false; + float _exponential = 0; + bool _accumulator = false; + bool _deadband = false; + bool _circleCorrection = true; + float _frequency = 25.0f; + Vehicle* _activeVehicle = nullptr; + + + bool _pollingStartedForCalibration = false; QString _name; bool _calibrated; @@ -207,28 +231,10 @@ protected: int _totalButtonCount; static int _transmitterMode; - bool _calibrationMode; - int* _rgAxisValues; - Calibration_t* _rgCalibration; - int _rgFunctionAxis[maxFunction]; + int _rgFunctionAxis[maxFunction] = {}; - bool* _rgButtonValues; QStringList _rgButtonActions; - quint16 _lastButtonBits; - - ThrottleMode_t _throttleMode; - - bool _negativeThrust; - - float _exponential; - bool _accumulator; - bool _deadband; - bool _circleCorrection; - float _frequency; - - Vehicle* _activeVehicle; - bool _pollingStartedForCalibration; MultiVehicleManager* _multiVehicleManager; @@ -261,6 +267,10 @@ private: static const char* _buttonActionPreviousStream; static const char* _buttonActionNextCamera; static const char* _buttonActionPreviousCamera; + static const char* _buttonActionTriggerCamera; + static const char* _buttonActionStartVideoRecord; + static const char* _buttonActionStopVideoRecord; + static const char* _buttonActionToggleVideoRecord; private slots: void _activeVehicleChanged(Vehicle* activeVehicle); diff --git a/src/VehicleSetup/JoystickConfig.qml b/src/VehicleSetup/JoystickConfig.qml index bdcaf70ee..1b8700468 100644 --- a/src/VehicleSetup/JoystickConfig.qml +++ b/src/VehicleSetup/JoystickConfig.qml @@ -27,7 +27,7 @@ SetupPage { pageName: qsTr("Joystick") pageDescription: qsTr("Joystick Setup is used to configure a calibrate joysticks.") - readonly property real _maxButtons: 16 + readonly property real _maxButtons: 64 Connections { target: joystickManager @@ -67,12 +67,12 @@ SetupPage { id: axisMonitorDisplayComponent Item { - property int axisValue: 0 - property int deadbandValue: 0 - property bool narrowIndicator: false - property color deadbandColor: "#8c161a" + property int axisValue: 0 + property int deadbandValue: 0 + property bool narrowIndicator: false + property color deadbandColor: "#8c161a" - property color __barColor: qgcPal.windowShade + property color __barColor: qgcPal.windowShade // Bar Rectangle { @@ -93,9 +93,9 @@ SetupPage { color: deadbandColor visible: controller.deadbandToggle - property real _percentDeadband: ((2 * deadbandValue) / (32768.0 * 2)) - property real _deadbandWidth: parent.width * _percentDeadband - property real _deadbandPosition: (parent.width - _deadbandWidth) / 2 + property real _percentDeadband: ((2 * deadbandValue) / (32768.0 * 2)) + property real _deadbandWidth: parent.width * _percentDeadband + property real _deadbandPosition: (parent.width - _deadbandWidth) / 2 } // Center point @@ -183,14 +183,12 @@ SetupPage { height: ScreenTools.defaultFontPixelHeight width: 100 sourceComponent: axisMonitorDisplayComponent - property bool mapped: controller.rollAxisMapped property bool reversed: controller.rollAxisReversed } Connections { target: _activeJoystick - onManualControl: rollLoader.item.axisValue = roll*32768.0 } } @@ -212,14 +210,12 @@ SetupPage { height: ScreenTools.defaultFontPixelHeight width: 100 sourceComponent: axisMonitorDisplayComponent - property bool mapped: controller.pitchAxisMapped property bool reversed: controller.pitchAxisReversed } Connections { target: _activeJoystick - onManualControl: pitchLoader.item.axisValue = pitch*32768.0 } } @@ -241,14 +237,12 @@ SetupPage { height: ScreenTools.defaultFontPixelHeight width: 100 sourceComponent: axisMonitorDisplayComponent - property bool mapped: controller.yawAxisMapped property bool reversed: controller.yawAxisReversed } Connections { target: _activeJoystick - onManualControl: yawLoader.item.axisValue = yaw*32768.0 } } @@ -270,17 +264,70 @@ SetupPage { height: ScreenTools.defaultFontPixelHeight width: 100 sourceComponent: axisMonitorDisplayComponent - property bool mapped: controller.throttleAxisMapped property bool reversed: controller.throttleAxisReversed } Connections { target: _activeJoystick - onManualControl: throttleLoader.item.axisValue = _activeJoystick.negativeThrust ? -throttle*32768.0 : (-2*throttle+1)*32768.0 } } + + Item { + width: parent.width + height: defaultTextHeight * 2 + + QGCLabel { + id: gimbalPitchLabel + width: ScreenTools.defaultFontPixelWidth * 10 + text: qsTr("Gimbal Pitch") + } + + Loader { + id: gimbalPitchLoader + anchors.left: gimbalPitchLabel.right + anchors.right: parent.right + height: ScreenTools.defaultFontPixelHeight + width: 100 + sourceComponent: axisMonitorDisplayComponent + property bool mapped: controller.gimbalPitchAxisMapped + property bool reversed: controller.gimbalPitchAxisReversed + } + + Connections { + target: _activeJoystick + onManualControl: gimbalPitchLoader.item.axisValue = gimbalPitch * 32768.0 + } + } + + Item { + width: parent.width + height: defaultTextHeight * 2 + + QGCLabel { + id: gimbalYawLabel + width: ScreenTools.defaultFontPixelWidth * 10 + text: qsTr("Gimbal Yaw") + } + + Loader { + id: gimbalYawLoader + anchors.left: gimbalYawLabel.right + anchors.right: parent.right + height: ScreenTools.defaultFontPixelHeight + width: 100 + sourceComponent: axisMonitorDisplayComponent + property bool mapped: controller.gimbalYawAxisMapped + property bool reversed: controller.gimbalYawAxisReversed + } + + Connections { + target: _activeJoystick + onManualControl: gimbalYawLoader.item.axisValue = gimbalYaw * 32768.0 + } + } + } // Column - Attitude Control labels // Command Buttons @@ -291,14 +338,12 @@ SetupPage { QGCButton { id: skipButton text: qsTr("Skip") - onClicked: controller.skipButtonClicked() } QGCButton { id: cancelButton text: qsTr("Cancel") - onClicked: controller.cancelButtonClicked() } @@ -306,7 +351,6 @@ SetupPage { id: nextButton primary: true text: qsTr("Calibrate") - onClicked: controller.nextButtonClicked() } } // Row - Buttons diff --git a/src/VehicleSetup/JoystickConfigController.cc b/src/VehicleSetup/JoystickConfigController.cc index 3ee4119eb..7745dddd4 100644 --- a/src/VehicleSetup/JoystickConfigController.cc +++ b/src/VehicleSetup/JoystickConfigController.cc @@ -44,23 +44,10 @@ const char* JoystickConfigController::_imagePitchUp = "joystickPitchUp.png const char* JoystickConfigController::_imagePitchDown = "joystickPitchDown.png"; JoystickConfigController::JoystickConfigController(void) - : _activeJoystick(NULL) - , _transmitterMode(2) - , _currentStep(-1) - , _axisCount(0) - , _rgAxisInfo(NULL) - , _axisValueSave(NULL) - , _axisRawValue(NULL) - , _calState(calStateAxisWait) - , _statusText(NULL) - , _cancelButton(NULL) - , _nextButton(NULL) - , _skipButton(NULL) - , _joystickManager(qgcApp()->toolbox()->joystickManager()) + : _joystickManager(qgcApp()->toolbox()->joystickManager()) { connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &JoystickConfigController::_activeJoystickChanged); - _activeJoystickChanged(_joystickManager->activeJoystick()); _resetInternalCalibrationValues(); } @@ -89,61 +76,69 @@ JoystickConfigController::~JoystickConfigController() /// @brief Returns the state machine entry for the specified state. const JoystickConfigController::stateMachineEntry* JoystickConfigController::_getStateMachineEntry(int step) { - static const char* msgBegin = "Allow all sticks to center as shown in diagram.\n\nClick Next to continue"; - static const char* msgThrottleUp = "Move the Throttle stick all the way up and hold it there..."; - static const char* msgThrottleDown = "Move the Throttle stick all the way down and hold it there..."; - static const char* msgYawLeft = "Move the Yaw stick all the way to the left and hold it there..."; - static const char* msgYawRight = "Move the Yaw stick all the way to the right and hold it there..."; - static const char* msgRollLeft = "Move the Roll stick all the way to the left and hold it there..."; - static const char* msgRollRight = "Move the Roll stick all the way to the right and hold it there..."; - static const char* msgPitchDown = "Move the Pitch stick all the way down and hold it there..."; - static const char* msgPitchUp = "Move the Pitch stick all the way up and hold it there..."; - static const char* msgPitchCenter = "Allow the Pitch stick to move back to center..."; - static const char* msgComplete = "All settings have been captured. Click Next to enable the joystick."; + static const char* msgBegin = "Allow all sticks to center as shown in diagram.\n\nClick Next to continue"; + static const char* msgThrottleUp = "Move the Throttle stick all the way up and hold it there..."; + static const char* msgThrottleDown = "Move the Throttle stick all the way down and hold it there..."; + static const char* msgYawLeft = "Move the Yaw stick all the way to the left and hold it there..."; + static const char* msgYawRight = "Move the Yaw stick all the way to the right and hold it there..."; + static const char* msgRollLeft = "Move the Roll stick all the way to the left and hold it there..."; + static const char* msgRollRight = "Move the Roll stick all the way to the right and hold it there..."; + static const char* msgPitchDown = "Move the Pitch stick all the way down and hold it there..."; + static const char* msgPitchUp = "Move the Pitch stick all the way up and hold it there..."; + static const char* msgPitchCenter = "Allow the Pitch stick to move back to center..."; + static const char* msgGimbalPitchDown = "Move the Gimbal Pitch control all the way down and hold it there..."; + static const char* msgGimbalPitchUp = "Move the Gimbal Pitch control all the way up and hold it there..."; + static const char* msgGimbalYawLeft = "Move the Gimbal Yaw control all the way to the left and hold it there..."; + static const char* msgGimbalYawRight = "Move the Gimbal Yaw control all the way to the right and hold it there..."; + static const char* msgComplete = "All settings have been captured. Click Next to enable the joystick."; static const stateMachineEntry rgStateMachine[] = { //Function - { Joystick::maxFunction, msgBegin, _imageCenter, &JoystickConfigController::_inputCenterWaitBegin, &JoystickConfigController::_saveAllTrims, NULL }, - { Joystick::throttleFunction, msgThrottleUp, _imageThrottleUp, &JoystickConfigController::_inputStickDetect, NULL, NULL }, - { Joystick::throttleFunction, msgThrottleDown, _imageThrottleDown, &JoystickConfigController::_inputStickMin, NULL, NULL }, - { Joystick::yawFunction, msgYawRight, _imageYawRight, &JoystickConfigController::_inputStickDetect, NULL, NULL }, - { Joystick::yawFunction, msgYawLeft, _imageYawLeft, &JoystickConfigController::_inputStickMin, NULL, NULL }, - { Joystick::rollFunction, msgRollRight, _imageRollRight, &JoystickConfigController::_inputStickDetect, NULL, NULL }, - { Joystick::rollFunction, msgRollLeft, _imageRollLeft, &JoystickConfigController::_inputStickMin, NULL, NULL }, - { Joystick::pitchFunction, msgPitchUp, _imagePitchUp, &JoystickConfigController::_inputStickDetect, NULL, NULL }, - { Joystick::pitchFunction, msgPitchDown, _imagePitchDown, &JoystickConfigController::_inputStickMin, NULL, NULL }, - { Joystick::pitchFunction, msgPitchCenter, _imageCenter, &JoystickConfigController::_inputCenterWait, NULL, NULL }, - { Joystick::maxFunction, msgComplete, _imageCenter, NULL, &JoystickConfigController::_writeCalibration, NULL }, + { Joystick::maxFunction, msgBegin, _imageCenter, &JoystickConfigController::_inputCenterWaitBegin, &JoystickConfigController::_saveAllTrims, nullptr, 0 }, + { Joystick::throttleFunction, msgThrottleUp, _imageThrottleUp, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 0 }, + { Joystick::throttleFunction, msgThrottleDown, _imageThrottleDown, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 0 }, + { Joystick::yawFunction, msgYawRight, _imageYawRight, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 1 }, + { Joystick::yawFunction, msgYawLeft, _imageYawLeft, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 1 }, + { Joystick::rollFunction, msgRollRight, _imageRollRight, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 2 }, + { Joystick::rollFunction, msgRollLeft, _imageRollLeft, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 2 }, + { Joystick::pitchFunction, msgPitchUp, _imagePitchUp, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 3 }, + { Joystick::pitchFunction, msgPitchDown, _imagePitchDown, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 3 }, + { Joystick::pitchFunction, msgPitchCenter, _imageCenter, &JoystickConfigController::_inputCenterWait, nullptr, nullptr, 3 }, + + { Joystick::gimbalPitchFunction, msgGimbalPitchUp, _imageThrottleUp, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 4 }, + { Joystick::gimbalPitchFunction, msgGimbalPitchDown, _imageThrottleDown, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 4 }, + { Joystick::gimbalYawFunction, msgGimbalYawRight, _imageYawRight, &JoystickConfigController::_inputStickDetect, nullptr, nullptr, 5 }, + { Joystick::gimbalYawFunction, msgGimbalYawLeft, _imageYawLeft, &JoystickConfigController::_inputStickMin, nullptr, nullptr, 5 }, + + { Joystick::maxFunction, msgComplete, _imageCenter, nullptr, &JoystickConfigController::_writeCalibration, nullptr, -1 }, }; - Q_ASSERT(step >=0 && step < (int)(sizeof(rgStateMachine) / sizeof(rgStateMachine[0]))); - + Q_ASSERT(step >=0 && step < static_cast((sizeof(rgStateMachine) / sizeof(rgStateMachine[0])))); return &rgStateMachine[step]; } -void JoystickConfigController::_advanceState(void) +void JoystickConfigController::_advanceState() { _currentStep++; + const stateMachineEntry* state = _getStateMachineEntry(_currentStep); + while (state->channelID > _axisCount) { + _currentStep++; + state = _getStateMachineEntry(_currentStep); + } _setupCurrentState(); } - /// @brief Sets up the state machine according to the current step from _currentStep. -void JoystickConfigController::_setupCurrentState(void) +void JoystickConfigController::_setupCurrentState() { - const stateMachineEntry* state = _getStateMachineEntry(_currentStep); - + const stateMachineEntry* state = _getStateMachineEntry(_currentStep); _statusText->setProperty("text", state->instructions); - _setHelpImage(state->image); - _stickDetectAxis = _axisNoAxis; _stickDetectSettleStarted = false; - _calSaveCurrentValues(); - - _nextButton->setEnabled(state->nextFn != NULL); - _skipButton->setEnabled(state->skipFn != NULL); + _nextButton->setEnabled(state->nextFn != nullptr); + _skipButton->setEnabled(state->skipFn != nullptr); } void JoystickConfigController::_axisValueChanged(int axis, int value) @@ -152,16 +147,12 @@ void JoystickConfigController::_axisValueChanged(int axis, int value) // We always update raw values _axisRawValue[axis] = value; emit axisValueChanged(axis, _axisRawValue[axis]); - - //qCDebug(JoystickConfigControllerLog) << "Raw value" << axis << value; - if (_currentStep == -1) { // Track the axis count by keeping track of how many axes we see - if (axis + 1 > (int)_axisCount) { + if (axis + 1 > static_cast(_axisCount)) { _axisCount = axis + 1; } } - if (_currentStep != -1) { const stateMachineEntry* state = _getStateMachineEntry(_currentStep); Q_ASSERT(state); @@ -172,7 +163,7 @@ void JoystickConfigController::_axisValueChanged(int axis, int value) } } -void JoystickConfigController::nextButtonClicked(void) +void JoystickConfigController::nextButtonClicked() { if (_currentStep == -1) { // Need to have enough channels @@ -189,10 +180,9 @@ void JoystickConfigController::nextButtonClicked(void) } } -void JoystickConfigController::skipButtonClicked(void) +void JoystickConfigController::skipButtonClicked() { Q_ASSERT(_currentStep != -1); - const stateMachineEntry* state = _getStateMachineEntry(_currentStep); Q_ASSERT(state); Q_ASSERT(state->skipFn); @@ -210,20 +200,17 @@ bool JoystickConfigController::getDeadbandToggle() { void JoystickConfigController::setDeadbandToggle(bool deadband) { _activeJoystick->setDeadband(deadband); - _signalAllAttitudeValueChanges(); - emit deadbandToggled(deadband); } -void JoystickConfigController::_saveAllTrims(void) +void JoystickConfigController::_saveAllTrims() { // We save all trims as the first step. At this point no axes are mapped but it should still // allow us to get good trims for the roll/pitch/yaw/throttle even though we don't know which // axis they are yet. As we continue through the process the other axes will get their // trims reset to correct values. - - for (int i=0; i<_axisCount; i++) { + for (int i = 0; i < _axisCount; i++) { qCDebug(JoystickConfigControllerLog) << "_saveAllTrims trim" << _axisRawValue[i]; _rgAxisInfo[i].axisTrim = _axisRawValue[i]; } @@ -233,7 +220,6 @@ void JoystickConfigController::_saveAllTrims(void) void JoystickConfigController::_axisDeadbandChanged(int axis, int value) { value = abs(value)<_calValidMaxValue?abs(value):_calValidMaxValue; - _rgAxisInfo[axis].deadband = value; emit axisDeadbandChanged(axis,value); qCDebug(JoystickConfigControllerLog) << "Axis:" << axis << "Deadband:" << _rgAxisInfo[axis].deadband; @@ -243,14 +229,11 @@ void JoystickConfigController::_axisDeadbandChanged(int axis, int value) void JoystickConfigController::_inputCenterWaitBegin(Joystick::AxisFunction_t function, int axis, int value) { Q_UNUSED(function); - //sensing deadband - if ((abs(value)*1.1f>_rgAxisInfo[axis].deadband)&&(_activeJoystick->deadband())) { //add 10% on top of existing deadband - _axisDeadbandChanged(axis,abs(value)*1.1f); + if ((abs(value) * 1.1f > _rgAxisInfo[axis].deadband) && (_activeJoystick->deadband())) { //add 10% on top of existing deadband + _axisDeadbandChanged(axis, static_cast(abs(value) * 1.1f)); } - _nextButton->setEnabled(true); - // FIXME: Doesn't wait for center } @@ -265,17 +248,13 @@ bool JoystickConfigController::_stickSettleComplete(int axis, int value) if (abs(_stickDetectValue - value) > _calSettleDelta) { // Stick is moving too much to consider stopped - qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete still moving, axis:_stickDetectValue:value" << axis << _stickDetectValue << value; - _stickDetectValue = value; _stickDetectSettleStarted = false; } else { // Stick is still positioned within the specified small range - if (_stickDetectSettleStarted) { // We have already started waiting - if (_stickDetectSettleElapsed.elapsed() > _stickDetectSettleMSecs) { // Stick has stayed positioned in one place long enough, detection is complete. qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete detection complete, axis:_stickDetectValue:value" << axis << _stickDetectValue << value; @@ -283,9 +262,7 @@ bool JoystickConfigController::_stickSettleComplete(int axis, int value) } } else { // Start waiting for the stick to stay settled for _stickDetectSettleWaitMSecs msecs - qCDebug(JoystickConfigControllerLog) << "_stickSettleComplete starting settle timer, axis:_stickDetectValue:value" << axis << _stickDetectValue << value; - _stickDetectSettleStarted = true; _stickDetectSettleElapsed.start(); } @@ -310,12 +287,9 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi if (_stickDetectAxis == _axisNoAxis) { // We have not detected enough movement on a axis yet - if (abs(_axisValueSave[axis] - value) > _calMoveDelta) { // Stick has moved far enough to consider it as being selected for the function - qCDebug(JoystickConfigControllerLog) << "_inputStickDetect starting settle wait, function:axis:value" << function << axis << value; - // Setup up to detect stick being pegged to min or max value _stickDetectAxis = axis; _stickDetectInitialValue = value; @@ -324,25 +298,19 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi } else if (axis == _stickDetectAxis) { if (_stickSettleComplete(axis, value)) { AxisInfo* info = &_rgAxisInfo[axis]; - // Stick detection is complete. Stick should be at max position. // Map the axis to the function _rgFunctionAxisMapping[function] = axis; info->function = function; - // Axis should be at max value, if it is below initial set point the the axis is reversed. info->reversed = value < _axisValueSave[axis]; - if (info->reversed) { _rgAxisInfo[axis].axisMin = value; } else { _rgAxisInfo[axis].axisMax = value; } - qCDebug(JoystickConfigControllerLog) << "_inputStickDetect saving values, function:axis:value:reversed:_axisValueSave" << function << axis << value << info->reversed << _axisValueSave[axis]; - _signalAllAttitudeValueChanges(); - _advanceState(); } } @@ -351,17 +319,14 @@ void JoystickConfigController::_inputStickDetect(Joystick::AxisFunction_t functi void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, int axis, int value) { qCDebug(JoystickConfigControllerLog) << "_inputStickMin function:axis:value" << function << axis << value; - if (!_validAxis(axis)) { qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount; return; } - // We only care about the axis mapped to the function we are working on if (_rgFunctionAxisMapping[function] != axis) { return; } - if (_stickDetectAxis == _axisNoAxis) { // Setup up to detect stick being pegged to extreme position if (_rgAxisInfo[axis].reversed) { @@ -381,19 +346,15 @@ void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, } } else { // We are waiting for the selected axis to settle out - if (_stickSettleComplete(axis, value)) { AxisInfo* info = &_rgAxisInfo[axis]; - // Stick detection is complete. Stick should be at min position. if (info->reversed) { _rgAxisInfo[axis].axisMax = value; } else { _rgAxisInfo[axis].axisMin = value; } - qCDebug(JoystickConfigControllerLog) << "_inputStickMin saving values, function:axis:value:reversed" << function << axis << value << info->reversed; - _advanceState(); } } @@ -402,7 +363,6 @@ void JoystickConfigController::_inputStickMin(Joystick::AxisFunction_t function, void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t function, int axis, int value) { qCDebug(JoystickConfigControllerLog) << "_inputCenterWait function:axis:value" << function << axis << value; - if (!_validAxis(axis)) { qCWarning(JoystickConfigControllerLog) << "Invalid axis axis:_axisCount" << axis << _axisCount; return; @@ -431,79 +391,71 @@ void JoystickConfigController::_inputCenterWait(Joystick::AxisFunction_t functio } /// @brief Resets internal calibration values to their initial state in preparation for a new calibration sequence. -void JoystickConfigController::_resetInternalCalibrationValues(void) +void JoystickConfigController::_resetInternalCalibrationValues() { - // Set all raw axiss to not reversed and center point values - for (int i=0; i<_axisCount; i++) { + // Set all raw axis to not reversed and center point values + for (int i = 0; i < _axisCount; i++) { struct AxisInfo* info = &_rgAxisInfo[i]; info->function = Joystick::maxFunction; info->reversed = false; info->deadband = 0; emit axisDeadbandChanged(i,info->deadband); - info->axisMin = JoystickConfigController::_calCenterPoint; - info->axisMax = JoystickConfigController::_calCenterPoint; + info->axisMin = JoystickConfigController::_calCenterPoint; + info->axisMax = JoystickConfigController::_calCenterPoint; info->axisTrim = JoystickConfigController::_calCenterPoint; } // Initialize attitude function mapping to function axis not set - for (size_t i=0; iactiveJoystick(); - // Initialize all function mappings to not set - - for (int i=0; i<_axisCount; i++) { + for (int i = 0; i < _axisCount; i++) { struct AxisInfo* info = &_rgAxisInfo[i]; info->function = Joystick::maxFunction; } - for (size_t i=0; igetCalibration(axis); - info->axisTrim = calibration.center; - info->axisMin = calibration.min; - info->axisMax = calibration.max; - info->reversed = calibration.reversed; - info->deadband = calibration.deadband; + info->axisTrim = calibration.center; + info->axisMin = calibration.min; + info->axisMax = calibration.max; + info->reversed = calibration.reversed; + info->deadband = calibration.deadband; emit axisDeadbandChanged(axis,info->deadband); - qCDebug(JoystickConfigControllerLog) << "Read settings name:axis:min:max:trim:reversed" << joystick->name() << axis << info->axisMin << info->axisMax << info->axisTrim << info->reversed; } - for (int function=0; functiongetFunctionAxis((Joystick::AxisFunction_t)function); - if(paramAxis >= 0) { + paramAxis = joystick->getFunctionAxis(static_cast(function)); + if(paramAxis >= 0 && paramAxis < _axisCount) { _rgFunctionAxisMapping[function] = paramAxis; - _rgAxisInfo[paramAxis].function = (Joystick::AxisFunction_t)function; + _rgAxisInfo[paramAxis].function = static_cast(function); } } _transmitterMode = joystick->getTXMode(); - _signalAllAttitudeValueChanges(); } /// @brief Validates the current settings against the calibration rules resetting values as necessary. -void JoystickConfigController::_validateCalibration(void) +void JoystickConfigController::_validateCalibration() { - for (int chan = 0; chan<_axisCount; chan++) { + for (int chan = 0; chan < _axisCount; chan++) { struct AxisInfo* info = &_rgAxisInfo[chan]; - if (chan < _axisCount) { // Validate Min/Max values. Although the axis appears as available we still may // not have good min/max/trim values for it. Set to defaults if needed. @@ -513,7 +465,6 @@ void JoystickConfigController::_validateCalibration(void) info->axisMax = _calDefaultMaxValue; info->axisTrim = info->axisMin + ((info->axisMax - info->axisMin) / 2); } - switch (_rgAxisInfo[chan].function) { case Joystick::throttleFunction: case Joystick::yawFunction: @@ -543,31 +494,26 @@ void JoystickConfigController::_validateCalibration(void) } } - /// @brief Saves the rc calibration values to the board parameters. -void JoystickConfigController::_writeCalibration(void) +void JoystickConfigController::_writeCalibration() { Joystick* joystick = _joystickManager->activeJoystick(); - _validateCalibration(); - for (int axis=0; axis<_axisCount; axis++) { + for (int axis = 0; axis < _axisCount; axis++) { Joystick::Calibration_t calibration; - - struct AxisInfo* info = &_rgAxisInfo[axis]; - - calibration.center = info->axisTrim; - calibration.min = info->axisMin; - calibration.max = info->axisMax; - calibration.reversed = info->reversed; - calibration.deadband = info->deadband; - + struct AxisInfo* info = &_rgAxisInfo[axis]; + calibration.center = info->axisTrim; + calibration.min = info->axisMin; + calibration.max = info->axisMax; + calibration.reversed = info->reversed; + calibration.deadband = info->deadband; joystick->setCalibration(axis, calibration); } // Write function mapping parameters - for (int function=0; functionsetFunctionAxis((Joystick::AxisFunction_t)function, _rgFunctionAxisMapping[function]); + for (int function = 0; function < Joystick::maxFunction; function++) { + joystick->setFunctionAxis(static_cast(function), _rgFunctionAxisMapping[function]); } _stopCalibration(); @@ -580,40 +526,34 @@ void JoystickConfigController::_writeCalibration(void) } /// @brief Starts the calibration process -void JoystickConfigController::_startCalibration(void) +void JoystickConfigController::_startCalibration() { _activeJoystick->setCalibrationMode(true); _resetInternalCalibrationValues(); - _nextButton->setProperty("text", "Next"); _cancelButton->setEnabled(true); - _currentStep = 0; _setupCurrentState(); emit calibratingChanged(); } /// @brief Cancels the calibration process, setting things back to initial state. -void JoystickConfigController::_stopCalibration(void) +void JoystickConfigController::_stopCalibration() { _currentStep = -1; - _activeJoystick->setCalibrationMode(false); _setInternalCalibrationValuesFromSettings(); - _statusText->setProperty("text", ""); - _nextButton->setProperty("text", tr("Calibrate")); _nextButton->setEnabled(true); _cancelButton->setEnabled(false); _skipButton->setEnabled(false); - _setHelpImage(_imageCenter); emit calibratingChanged(); } /// @brief Saves the current axis values, so that we can detect when the use moves an input. -void JoystickConfigController::_calSaveCurrentValues(void) +void JoystickConfigController::_calSaveCurrentValues() { qCDebug(JoystickConfigControllerLog) << "_calSaveCurrentValues"; for (int i = 0; i < _axisCount; i++) { @@ -622,7 +562,7 @@ void JoystickConfigController::_calSaveCurrentValues(void) } /// @brief Set up the Save state of calibration. -void JoystickConfigController::_calSave(void) +void JoystickConfigController::_calSave() { _calState = calStateSave; @@ -668,34 +608,7 @@ void JoystickConfigController::_setHelpImage(const char* imageFile) emit imageHelpChanged(file); } -int JoystickConfigController::axisCount(void) -{ - return _axisCount; -} - - - -bool JoystickConfigController::rollAxisMapped(void) -{ - return _rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis; -} - -bool JoystickConfigController::pitchAxisMapped(void) -{ - return _rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis; -} - -bool JoystickConfigController::yawAxisMapped(void) -{ - return _rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis; -} - -bool JoystickConfigController::throttleAxisMapped(void) -{ - return _rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis; -} - -bool JoystickConfigController::rollAxisReversed(void) +bool JoystickConfigController::rollAxisReversed() { if (_rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis) { return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::rollFunction]].reversed; @@ -704,7 +617,7 @@ bool JoystickConfigController::rollAxisReversed(void) } } -bool JoystickConfigController::pitchAxisReversed(void) +bool JoystickConfigController::pitchAxisReversed() { if (_rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis) { return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::pitchFunction]].reversed; @@ -713,7 +626,7 @@ bool JoystickConfigController::pitchAxisReversed(void) } } -bool JoystickConfigController::yawAxisReversed(void) +bool JoystickConfigController::yawAxisReversed() { if (_rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis) { return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::yawFunction]].reversed; @@ -722,7 +635,7 @@ bool JoystickConfigController::yawAxisReversed(void) } } -bool JoystickConfigController::throttleAxisReversed(void) +bool JoystickConfigController::throttleAxisReversed() { if (_rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis) { return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::throttleFunction]].reversed; @@ -731,6 +644,24 @@ bool JoystickConfigController::throttleAxisReversed(void) } } +bool JoystickConfigController::gimbalPitchAxisReversed() +{ + if (_rgFunctionAxisMapping[Joystick::gimbalPitchFunction] != _axisNoAxis) { + return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::gimbalPitchFunction]].reversed; + } else { + return false; + } +} + +bool JoystickConfigController::gimbalYawAxisReversed() +{ + if (_rgFunctionAxisMapping[Joystick::gimbalYawFunction] != _axisNoAxis) { + return _rgAxisInfo[_rgFunctionAxisMapping[Joystick::gimbalYawFunction]].reversed; + } else { + return false; + } +} + void JoystickConfigController::setTransmitterMode(int mode) { if (mode > 0 && mode <= 4) { @@ -745,17 +676,21 @@ void JoystickConfigController::setTransmitterMode(int mode) } } -void JoystickConfigController::_signalAllAttitudeValueChanges(void) +void JoystickConfigController::_signalAllAttitudeValueChanges() { emit rollAxisMappedChanged(rollAxisMapped()); emit pitchAxisMappedChanged(pitchAxisMapped()); emit yawAxisMappedChanged(yawAxisMapped()); emit throttleAxisMappedChanged(throttleAxisMapped()); - + emit gimbalPitchAxisMappedChanged(gimbalPitchAxisMapped()); + emit gimbalYawAxisMappedChanged(gimbalYawAxisMapped()); + emit rollAxisReversedChanged(rollAxisReversed()); emit pitchAxisReversedChanged(pitchAxisReversed()); emit yawAxisReversedChanged(yawAxisReversed()); emit throttleAxisReversedChanged(throttleAxisReversed()); + emit gimbalPitchAxisReversedChanged(pitchAxisReversed()); + emit gimbalYawAxisReversedChanged(yawAxisReversed()); emit transmitterModeChanged(_transmitterMode); } @@ -772,7 +707,7 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick) delete[] _axisValueSave; delete[] _axisRawValue; _axisCount = 0; - _activeJoystick = NULL; + _activeJoystick = nullptr; } if (joystick) { @@ -781,10 +716,10 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick) _stopCalibration(); } _activeJoystick->setCalibrationMode(false); - _axisCount = _activeJoystick->axisCount(); - _rgAxisInfo = new struct AxisInfo[_axisCount]; - _axisValueSave = new int[_axisCount]; - _axisRawValue = new int[_axisCount]; + _axisCount = _activeJoystick->axisCount(); + _rgAxisInfo = new struct AxisInfo[_axisCount]; + _axisValueSave = new int[_axisCount]; + _axisRawValue = new int[_axisCount]; _setInternalCalibrationValuesFromSettings(); connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged); } diff --git a/src/VehicleSetup/JoystickConfigController.h b/src/VehicleSetup/JoystickConfigController.h index e2b997dc3..1fb0a2946 100644 --- a/src/VehicleSetup/JoystickConfigController.h +++ b/src/VehicleSetup/JoystickConfigController.h @@ -47,70 +47,84 @@ public: Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton) Q_PROPERTY(QQuickItem* skipButton MEMBER _skipButton) - Q_PROPERTY(bool rollAxisMapped READ rollAxisMapped NOTIFY rollAxisMappedChanged) - Q_PROPERTY(bool pitchAxisMapped READ pitchAxisMapped NOTIFY pitchAxisMappedChanged) - Q_PROPERTY(bool yawAxisMapped READ yawAxisMapped NOTIFY yawAxisMappedChanged) - Q_PROPERTY(bool throttleAxisMapped READ throttleAxisMapped NOTIFY throttleAxisMappedChanged) + Q_PROPERTY(bool rollAxisMapped READ rollAxisMapped NOTIFY rollAxisMappedChanged) + Q_PROPERTY(bool pitchAxisMapped READ pitchAxisMapped NOTIFY pitchAxisMappedChanged) + Q_PROPERTY(bool yawAxisMapped READ yawAxisMapped NOTIFY yawAxisMappedChanged) + Q_PROPERTY(bool throttleAxisMapped READ throttleAxisMapped NOTIFY throttleAxisMappedChanged) - Q_PROPERTY(int rollAxisReversed READ rollAxisReversed NOTIFY rollAxisReversedChanged) - Q_PROPERTY(int pitchAxisReversed READ pitchAxisReversed NOTIFY pitchAxisReversedChanged) - Q_PROPERTY(int yawAxisReversed READ yawAxisReversed NOTIFY yawAxisReversedChanged) - Q_PROPERTY(int throttleAxisReversed READ throttleAxisReversed NOTIFY throttleAxisReversedChanged) - - Q_PROPERTY(bool deadbandToggle READ getDeadbandToggle WRITE setDeadbandToggle NOTIFY deadbandToggled) + Q_PROPERTY(bool gimbalPitchAxisMapped READ gimbalPitchAxisMapped NOTIFY gimbalPitchAxisMappedChanged) + Q_PROPERTY(bool gimbalYawAxisMapped READ gimbalYawAxisMapped NOTIFY gimbalYawAxisMappedChanged) + + Q_PROPERTY(int rollAxisReversed READ rollAxisReversed NOTIFY rollAxisReversedChanged) + Q_PROPERTY(int pitchAxisReversed READ pitchAxisReversed NOTIFY pitchAxisReversedChanged) + Q_PROPERTY(int yawAxisReversed READ yawAxisReversed NOTIFY yawAxisReversedChanged) + Q_PROPERTY(int throttleAxisReversed READ throttleAxisReversed NOTIFY throttleAxisReversedChanged) + + Q_PROPERTY(int gimbalPitchAxisReversed READ gimbalPitchAxisReversed NOTIFY gimbalPitchAxisReversedChanged) + Q_PROPERTY(int gimbalYawAxisReversed READ gimbalYawAxisReversed NOTIFY gimbalYawAxisReversedChanged) + + Q_PROPERTY(bool deadbandToggle READ getDeadbandToggle WRITE setDeadbandToggle NOTIFY deadbandToggled) Q_PROPERTY(int transmitterMode READ transmitterMode WRITE setTransmitterMode NOTIFY transmitterModeChanged) Q_PROPERTY(QString imageHelp MEMBER _imageHelp NOTIFY imageHelpChanged) Q_PROPERTY(bool calibrating READ calibrating NOTIFY calibratingChanged) - Q_INVOKABLE void cancelButtonClicked(void); - Q_INVOKABLE void skipButtonClicked(void); - Q_INVOKABLE void nextButtonClicked(void); - Q_INVOKABLE void start(void); - Q_INVOKABLE void setDeadbandValue(int axis, int value); + Q_INVOKABLE void cancelButtonClicked (); + Q_INVOKABLE void skipButtonClicked (); + Q_INVOKABLE void nextButtonClicked (); + Q_INVOKABLE void start (); + Q_INVOKABLE void setDeadbandValue (int axis, int value); - bool rollAxisMapped(void); - bool pitchAxisMapped(void); - bool yawAxisMapped(void); - bool throttleAxisMapped(void); - - bool rollAxisReversed(void); - bool pitchAxisReversed(void); - bool yawAxisReversed(void); - bool throttleAxisReversed(void); - - bool getDeadbandToggle(void); - void setDeadbandToggle(bool); + bool rollAxisMapped () { return _rgFunctionAxisMapping[Joystick::rollFunction] != _axisNoAxis; } + bool pitchAxisMapped () { return _rgFunctionAxisMapping[Joystick::pitchFunction] != _axisNoAxis; } + bool yawAxisMapped () { return _rgFunctionAxisMapping[Joystick::yawFunction] != _axisNoAxis; } + bool throttleAxisMapped () { return _rgFunctionAxisMapping[Joystick::throttleFunction] != _axisNoAxis; } + bool gimbalPitchAxisMapped () { return _rgFunctionAxisMapping[Joystick::gimbalPitchFunction] != _axisNoAxis; } + bool gimbalYawAxisMapped () { return _rgFunctionAxisMapping[Joystick::gimbalYawFunction] != _axisNoAxis; } - int axisCount(void); + bool rollAxisReversed (); + bool pitchAxisReversed (); + bool yawAxisReversed (); + bool throttleAxisReversed (); + bool gimbalPitchAxisReversed (); + bool gimbalYawAxisReversed (); - int transmitterMode(void) { return _transmitterMode; } - void setTransmitterMode(int mode); + bool getDeadbandToggle (); + void setDeadbandToggle (bool); - bool calibrating(void) { return _currentStep != -1; } + int axisCount () { return _axisCount; } + + int transmitterMode () { return _transmitterMode; } + void setTransmitterMode (int mode); + + bool calibrating () { return _currentStep != -1; } signals: - void axisValueChanged(int axis, int value); - void axisDeadbandChanged(int axis, int value); + void axisValueChanged (int axis, int value); + void axisDeadbandChanged (int axis, int value); - void rollAxisMappedChanged(bool mapped); - void pitchAxisMappedChanged(bool mapped); - void yawAxisMappedChanged(bool mapped); - void throttleAxisMappedChanged(bool mapped); + void rollAxisMappedChanged (bool mapped); + void pitchAxisMappedChanged (bool mapped); + void yawAxisMappedChanged (bool mapped); + void throttleAxisMappedChanged (bool mapped); + void gimbalPitchAxisMappedChanged (bool mapped); + void gimbalYawAxisMappedChanged (bool mapped); - void rollAxisReversedChanged(bool reversed); - void pitchAxisReversedChanged(bool reversed); - void yawAxisReversedChanged(bool reversed); - void throttleAxisReversedChanged(bool reversed); + void rollAxisReversedChanged (bool reversed); + void pitchAxisReversedChanged (bool reversed); + void yawAxisReversedChanged (bool reversed); + void throttleAxisReversedChanged (bool reversed); + void gimbalPitchAxisReversedChanged (bool reversed); + void gimbalYawAxisReversedChanged (bool reversed); - void deadbandToggled(bool value); + void deadbandToggled (bool value); - void imageHelpChanged(QString source); - void transmitterModeChanged(int mode); - void calibratingChanged(void); + void imageHelpChanged (QString source); + void transmitterModeChanged (int mode); + void calibratingChanged (); // @brief Signalled when in unit test mode and a message box should be displayed by the next button - void nextButtonMessageBoxDisplayed(void); + void nextButtonMessageBoxDisplayed (); private slots: void _activeJoystickChanged(Joystick* joystick); @@ -139,6 +153,7 @@ private: inputFn rcInputFn; buttonFn nextFn; buttonFn skipFn; + int channelID; }; /// @brief A set of information associated with a radio axis. @@ -151,45 +166,45 @@ private: int deadband; ///< Deadband }; - Joystick* _activeJoystick; + Joystick* _activeJoystick = nullptr; - int _transmitterMode; - int _currentStep; ///< Current step of state machine + int _transmitterMode = 2; + int _currentStep = -1; ///< Current step of state machine const struct stateMachineEntry* _getStateMachineEntry(int step); - void _advanceState(void); - void _setupCurrentState(void); + void _advanceState (); + void _setupCurrentState (); - bool _validAxis(int axis); + bool _validAxis (int axis); void _inputCenterWaitBegin (Joystick::AxisFunction_t function, int axis, int value); void _inputStickDetect (Joystick::AxisFunction_t function, int axis, int value); void _inputStickMin (Joystick::AxisFunction_t function, int axis, int value); void _inputCenterWait (Joystick::AxisFunction_t function, int axis, int value); - void _switchDetect(Joystick::AxisFunction_t function, int axis, int value, bool moveToNextStep); + void _switchDetect (Joystick::AxisFunction_t function, int axis, int value, bool moveToNextStep); - void _saveFlapsDown(void); - void _skipFlaps(void); - void _saveAllTrims(void); + void _saveFlapsDown (); + void _skipFlaps (); + void _saveAllTrims (); - bool _stickSettleComplete(int axis, int value); + bool _stickSettleComplete (int axis, int value); - void _validateCalibration(void); - void _writeCalibration(void); - void _resetInternalCalibrationValues(void); - void _setInternalCalibrationValuesFromSettings(void); + void _validateCalibration (); + void _writeCalibration (); + void _resetInternalCalibrationValues(); + void _setInternalCalibrationValuesFromSettings(); - void _startCalibration(void); - void _stopCalibration(void); - void _calSave(void); + void _startCalibration (); + void _stopCalibration (); + void _calSave (); - void _calSaveCurrentValues(void); + void _calSaveCurrentValues (); void _setHelpImage(const char* imageFile); - void _signalAllAttitudeValueChanges(void); + void _signalAllAttitudeValueChanges(); // Member variables @@ -210,20 +225,21 @@ private: int _rgFunctionAxisMapping[Joystick::maxFunction]; ///< Maps from joystick function to axis index. _axisMax indicates axis not set for this function. - static const int _attitudeControls = 5; - - int _axisCount; ///< Number of actual joystick axes available - static const int _axisNoAxis = -1; ///< Signals no axis set - static const int _axisMinimum = 4; ///< Minimum numner of joystick axes required to run PX4 - struct AxisInfo* _rgAxisInfo; ///< Information associated with each axis - int* _axisValueSave; ///< Saved values prior to detecting axis movement - int* _axisRawValue; ///< Current set of raw axis values + static const int _attitudeControls = 5; + + int _axisCount = 0; ///< Number of actual joystick axes available + static const int _axisNoAxis = -1; ///< Signals no axis set + static const int _axisMinimum = 4; ///< Minimum numner of joystick axes required to run PX4 + struct AxisInfo* _rgAxisInfo = nullptr; ///< Information associated with each axis + int* _axisValueSave = nullptr; ///< Saved values prior to detecting axis movement + int* _axisRawValue = nullptr; ///< Current set of raw axis values - enum calStates _calState; ///< Current calibration state - int _calStateCurrentAxis; ///< Current axis being worked on in calStateIdentify and calStateDetectInversion - bool _calStateAxisComplete; ///< Work associated with current axis is complete - int _calStateIdentifyOldMapping; ///< Previous mapping for axis being currently identified - int _calStateReverseOldMapping; ///< Previous mapping for axis being currently used to detect inversion + enum calStates _calState = calStateAxisWait; ///< Current calibration state + + int _calStateCurrentAxis; ///< Current axis being worked on in calStateIdentify and calStateDetectInversion + bool _calStateAxisComplete; ///< Work associated with current axis is complete + int _calStateIdentifyOldMapping; ///< Previous mapping for axis being currently identified + int _calStateReverseOldMapping; ///< Previous mapping for axis being currently used to detect inversion static const int _calCenterPoint; static const int _calValidMinValue; @@ -241,11 +257,11 @@ private: bool _stickDetectSettleStarted; QTime _stickDetectSettleElapsed; static const int _stickDetectSettleMSecs; - - QQuickItem* _statusText; - QQuickItem* _cancelButton; - QQuickItem* _nextButton; - QQuickItem* _skipButton; + + QQuickItem* _statusText = nullptr; + QQuickItem* _cancelButton = nullptr; + QQuickItem* _nextButton = nullptr; + QQuickItem* _skipButton = nullptr; QString _imageHelp; -- 2.22.0