Skip to content

Q
qgroundcontrol
Commit 3ee54e56 authored by Gus Grubba's avatar Gus Grubba
Browse files
Options

Fix button state handling 

Support up to 64 buttons, not limit to 16
Send a single button command and not a stream of commands at 25Hz
Send button commands through signals (it's coming from a different thread)
Hide gimbal support through analog channels (there are no MAVLink messages to handle it)
parent e28abddc
Hide whitespace changes
Inline Side-by-side
Showing with 330 additions and 246 deletions
src/Joystick/Joystick.cc
View file @ 3ee54e56
...@@ -23,7 +23,7 @@ QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog") ...@@ -23,7 +23,7 @@ QGC_LOGGING_CATEGORY(JoystickLog, "JoystickLog")
QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog") QGC_LOGGING_CATEGORY(JoystickValuesLog, "JoystickValuesLog")
const char* Joystick::_settingsGroup = "Joysticks"; const char* Joystick::_settingsGroup = "Joysticks";
const char* Joystick::_calibratedSettingsKey = "Calibrated2"; // Increment number to force recalibration const char* Joystick::_calibratedSettingsKey = "Calibrated3"; // Increment number to force recalibration
const char* Joystick::_buttonActionSettingsKey = "ButtonActionName%1"; const char* Joystick::_buttonActionSettingsKey = "ButtonActionName%1";
const char* Joystick::_throttleModeSettingsKey = "ThrottleMode"; const char* Joystick::_throttleModeSettingsKey = "ThrottleMode";
const char* Joystick::_exponentialSettingsKey = "Exponential"; const char* Joystick::_exponentialSettingsKey = "Exponential";
...@@ -37,6 +37,7 @@ const char* Joystick::_multiRotorTXModeSettingsKey = "TXMode_MultiRotor"; ...@@ -37,6 +37,7 @@ const char* Joystick::_multiRotorTXModeSettingsKey = "TXMode_MultiRotor";
const char* Joystick::_roverTXModeSettingsKey = "TXMode_Rover"; const char* Joystick::_roverTXModeSettingsKey = "TXMode_Rover";
const char* Joystick::_vtolTXModeSettingsKey = "TXMode_VTOL"; const char* Joystick::_vtolTXModeSettingsKey = "TXMode_VTOL";
const char* Joystick::_submarineTXModeSettingsKey = "TXMode_Submarine"; const char* Joystick::_submarineTXModeSettingsKey = "TXMode_Submarine";
const char* Joystick::_gimbalSettingsKey = "GimbalEnabled";
const char* Joystick::_buttonActionArm = QT_TR_NOOP("Arm"); const char* Joystick::_buttonActionArm = QT_TR_NOOP("Arm");
const char* Joystick::_buttonActionDisarm = QT_TR_NOOP("Disarm"); const char* Joystick::_buttonActionDisarm = QT_TR_NOOP("Disarm");
...@@ -81,13 +82,13 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC ...@@ -81,13 +82,13 @@ Joystick::Joystick(const QString& name, int axisCount, int buttonCount, int hatC
{ {
_rgAxisValues = new int[_axisCount]; _rgAxisValues = new int[_axisCount];
_rgCalibration = new Calibration_t[_axisCount]; _rgCalibration = new Calibration_t[_axisCount];
_rgButtonValues = new bool[_totalButtonCount]; _rgButtonValues = new uint8_t[_totalButtonCount];
for (int i = 0; i < _axisCount; i++) { for (int i = 0; i < _axisCount; i++) {
_rgAxisValues[i] = 0; _rgAxisValues[i] = 0;
} }
for (int i = 0; i < _totalButtonCount; i++) { for (int i = 0; i < _totalButtonCount; i++) {
_rgButtonValues[i] = false; _rgButtonValues[i] = BUTTON_UP;
} }
_updateTXModeSettingsKey(_multiVehicleManager->activeVehicle()); _updateTXModeSettingsKey(_multiVehicleManager->activeVehicle());
...@@ -169,12 +170,10 @@ void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle) ...@@ -169,12 +170,10 @@ void Joystick::_updateTXModeSettingsKey(Vehicle* activeVehicle)
void Joystick::_activeVehicleChanged(Vehicle* activeVehicle) void Joystick::_activeVehicleChanged(Vehicle* activeVehicle)
{ {
_updateTXModeSettingsKey(activeVehicle); _updateTXModeSettingsKey(activeVehicle);
if(activeVehicle) { if(activeVehicle) {
QSettings settings; QSettings settings;
settings.beginGroup(_settingsGroup); settings.beginGroup(_settingsGroup);
int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt(); int mode = settings.value(_txModeSettingsKey, activeVehicle->firmwarePlugin()->defaultJoystickTXMode()).toInt();
setTXMode(mode); setTXMode(mode);
} }
} }
...@@ -201,6 +200,7 @@ void Joystick::_loadSettings() ...@@ -201,6 +200,7 @@ void Joystick::_loadSettings()
_deadband = settings.value(_deadbandSettingsKey, false).toBool(); _deadband = settings.value(_deadbandSettingsKey, false).toBool();
_frequency = settings.value(_frequencySettingsKey, 25.0f).toFloat(); _frequency = settings.value(_frequencySettingsKey, 25.0f).toFloat();
_circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool(); _circleCorrection = settings.value(_circleCorrectionSettingsKey, false).toBool();
_gimbalEnabled = settings.value(_gimbalSettingsKey, false).toBool();
_throttleMode = static_cast<ThrottleMode_t>(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk)); _throttleMode = static_cast<ThrottleMode_t>(settings.value(_throttleModeSettingsKey, ThrottleModeDownZero).toInt(&convertOk));
badSettings |= !convertOk; badSettings |= !convertOk;
...@@ -277,6 +277,7 @@ void Joystick::_saveSettings() ...@@ -277,6 +277,7 @@ void Joystick::_saveSettings()
settings.setValue(_deadbandSettingsKey, _deadband); settings.setValue(_deadbandSettingsKey, _deadband);
settings.setValue(_frequencySettingsKey, _frequency); settings.setValue(_frequencySettingsKey, _frequency);
settings.setValue(_throttleModeSettingsKey, _throttleMode); settings.setValue(_throttleModeSettingsKey, _throttleMode);
settings.setValue(_gimbalSettingsKey, _gimbalEnabled);
settings.setValue(_circleCorrectionSettingsKey, _circleCorrection); settings.setValue(_circleCorrectionSettingsKey, _circleCorrection);
qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode; qCDebug(JoystickLog) << "_saveSettings calibrated:throttlemode:deadband:txmode" << _calibrated << _throttleMode << _deadband << _circleCorrection << _transmitterMode;
...@@ -410,7 +411,7 @@ float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDead ...@@ -410,7 +411,7 @@ float Joystick::_adjustRange(int value, Calibration_t calibration, bool withDead
} }
void Joystick::run(void) void Joystick::run()
{ {
_open(); _open();
...@@ -428,8 +429,11 @@ void Joystick::run(void) ...@@ -428,8 +429,11 @@ void Joystick::run(void)
// Update buttons // Update buttons
for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) { for (int buttonIndex = 0; buttonIndex < _buttonCount; buttonIndex++) {
bool newButtonValue = _getButton(buttonIndex); bool newButtonValue = _getButton(buttonIndex);
if (newButtonValue != _rgButtonValues[buttonIndex]) { if (newButtonValue && _rgButtonValues[buttonIndex] == BUTTON_UP) {
_rgButtonValues[buttonIndex] = newButtonValue; _rgButtonValues[buttonIndex] = BUTTON_DOWN;
emit rawButtonPressedChanged(buttonIndex, newButtonValue);
} else if (!newButtonValue && _rgButtonValues[buttonIndex] != BUTTON_UP) {
_rgButtonValues[buttonIndex] = BUTTON_UP;
emit rawButtonPressedChanged(buttonIndex, newButtonValue); emit rawButtonPressedChanged(buttonIndex, newButtonValue);
} }
} }
...@@ -441,9 +445,12 @@ void Joystick::run(void) ...@@ -441,9 +445,12 @@ void Joystick::run(void)
// Create new index value that includes the normal button list // Create new index value that includes the normal button list
int rgButtonValueIndex = hatIndex*numHatButtons + hatButtonIndex + _buttonCount; int rgButtonValueIndex = hatIndex*numHatButtons + hatButtonIndex + _buttonCount;
// Get hat value from joystick // Get hat value from joystick
bool newButtonValue = _getHat(hatIndex,hatButtonIndex); bool newButtonValue = _getHat(hatIndex, hatButtonIndex);
if (newButtonValue != _rgButtonValues[rgButtonValueIndex]) { if (newButtonValue && _rgButtonValues[rgButtonValueIndex] == BUTTON_UP) {
_rgButtonValues[rgButtonValueIndex] = newButtonValue; _rgButtonValues[rgButtonValueIndex] = BUTTON_DOWN;
emit rawButtonPressedChanged(rgButtonValueIndex, newButtonValue);
} else if (!newButtonValue && _rgButtonValues[rgButtonValueIndex] != BUTTON_UP) {
_rgButtonValues[rgButtonValueIndex] = BUTTON_UP;
emit rawButtonPressedChanged(rgButtonValueIndex, newButtonValue); emit rawButtonPressedChanged(rgButtonValueIndex, newButtonValue);
} }
} }
...@@ -451,31 +458,38 @@ void Joystick::run(void) ...@@ -451,31 +458,38 @@ void Joystick::run(void)
if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) { if (_activeVehicle->joystickEnabled() && !_calibrationMode && _calibrated) {
int axis = _rgFunctionAxis[rollFunction]; int axis = _rgFunctionAxis[rollFunction];
float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); float roll = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
axis = _rgFunctionAxis[pitchFunction]; axis = _rgFunctionAxis[pitchFunction];
float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband); float pitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _deadband);
axis = _rgFunctionAxis[yawFunction]; axis = _rgFunctionAxis[yawFunction];
float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); float yaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
axis = _rgFunctionAxis[throttleFunction]; axis = _rgFunctionAxis[throttleFunction];
float throttle = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_deadband); float throttle = _adjustRange(_rgAxisValues[axis],_rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_deadband);
axis = _rgFunctionAxis[gimbalPitchFunction]; float gimbalPitch = 0.0f;
float gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); float gimbalYaw = 0.0f;
axis = _rgFunctionAxis[gimbalYawFunction]; if(_axisCount > 4) {
float gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband); axis = _rgFunctionAxis[gimbalPitchFunction];
gimbalPitch = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
}
if(_axisCount > 5) {
axis = _rgFunctionAxis[gimbalYawFunction];
gimbalYaw = _adjustRange(_rgAxisValues[axis], _rgCalibration[axis],_deadband);
}
if ( _accumulator ) { if (_accumulator) {
static float throttle_accu = 0.f; static float throttle_accu = 0.f;
throttle_accu += throttle*(40/1000.f); //for throttle to change from min to max it will take 1000ms (40ms is a loop time) throttle_accu += throttle * (40 / 1000.f); //for throttle to change from min to max it will take 1000ms (40ms is a loop time)
throttle_accu = std::max(static_cast<float>(-1.f), std::min(throttle_accu, static_cast<float>(1.f))); throttle_accu = std::max(static_cast<float>(-1.f), std::min(throttle_accu, static_cast<float>(1.f)));
throttle = throttle_accu; throttle = throttle_accu;
} }
if ( _circleCorrection ) { if (_circleCorrection) {
float roll_limited = std::max(static_cast<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4))); float roll_limited = std::max(static_cast<float>(-M_PI_4), std::min(roll, static_cast<float>(M_PI_4)));
float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4))); float pitch_limited = std::max(static_cast<float>(-M_PI_4), std::min(pitch, static_cast<float>(M_PI_4)));
float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4))); float yaw_limited = std::max(static_cast<float>(-M_PI_4), std::min(yaw, static_cast<float>(M_PI_4)));
...@@ -490,8 +504,7 @@ void Joystick::run(void) ...@@ -490,8 +504,7 @@ void Joystick::run(void)
if ( _exponential < -0.01f) { if ( _exponential < -0.01f) {
// Exponential (0% to -50% range like most RC radios) // Exponential (0% to -50% range like most RC radios)
//_exponential is set by a slider in joystickConfigAdvanced.qml // _exponential is set by a slider in joystickConfigAdvanced.qml
// Calculate new RPY with exponential applied // 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; pitch = -_exponential*powf(pitch,3) + (1+_exponential)*pitch;
...@@ -507,36 +520,40 @@ void Joystick::run(void) ...@@ -507,36 +520,40 @@ void Joystick::run(void)
throttle = (throttle + 1.0f) / 2.0f; throttle = (throttle + 1.0f) / 2.0f;
} }
// Set up button pressed information //-- Process button press
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 = static_cast<quint16>(1 << buttonIndex); //-- This button just went down
if(_rgButtonValues[buttonIndex] == BUTTON_DOWN) {
if (!_rgButtonValues[buttonIndex]) { //-- Flag it as processed
// Button up, just record it _rgButtonValues[buttonIndex] = BUTTON_REPEAT;
newButtonBits |= buttonBit; //-- Process button
} else { QString buttonAction =_rgButtonActions[buttonIndex];
if (_lastButtonBits & buttonBit) { qCDebug(JoystickLog) << "button triggered" << buttonIndex << buttonAction;
// Button was up last time through, but is now down which indicates a button press if (!buttonAction.isEmpty()) {
qCDebug(JoystickLog) << "button triggered" << buttonIndex; _buttonAction(buttonAction);
QString buttonAction =_rgButtonActions[buttonIndex];
if (!buttonAction.isEmpty()) {
_buttonAction(buttonAction);
}
} }
}
}
// Set up button bitmap
quint64 buttonPressedBits = 0; // Buttons pressed for manualControl signal
for (int buttonIndex = 0; buttonIndex < _totalButtonCount; buttonIndex++) {
quint64 buttonBit = static_cast<quint64>(1 << buttonIndex);
if (_rgButtonValues[buttonIndex] != BUTTON_UP) {
// Mark the button as pressed as long as its pressed // Mark the button as pressed as long as its pressed
buttonPressedBits |= buttonBit; buttonPressedBits |= buttonBit;
} }
} }
_lastButtonBits = newButtonBits;
qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle:gimbalPitch:gimbalYaw" << name() << roll << -pitch << yaw << throttle << gimbalPitch << gimbalYaw; qCDebug(JoystickValuesLog) << "name:roll:pitch:yaw:throttle:gimbalPitch:gimbalYaw" << name() << roll << -pitch << yaw << throttle << gimbalPitch << gimbalYaw;
// NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub. // NOTE: The buttonPressedBits going to MANUAL_CONTROL are currently used by ArduSub (and it only handles 16 bits)
emit manualControl(roll, -pitch, yaw, throttle, buttonPressedBits, _activeVehicle->joystickMode()); uint16_t shortButtons = static_cast<uint16_t>(buttonPressedBits & 0xFFFF);
emit manualControl(roll, -pitch, yaw, throttle, shortButtons, _activeVehicle->joystickMode());
if(_activeVehicle && _axisCount > 4 && _gimbalEnabled) {
//-- TODO: There is nothing consuming this as there are no messages to handle gimbal
// the way MANUAL_CONTROL handles the other channels.
emit manualControlGimbal((gimbalPitch + 1.0f) / 2.0f * 90.0f, gimbalYaw * 180.0f);
}
} }
// Sleep. Update rate of joystick is by default 25 Hz // Sleep. Update rate of joystick is by default 25 Hz
...@@ -550,36 +567,40 @@ void Joystick::run(void) ...@@ -550,36 +567,40 @@ void Joystick::run(void)
void Joystick::startPolling(Vehicle* vehicle) void Joystick::startPolling(Vehicle* vehicle)
{ {
if (vehicle) { if (vehicle) {
// If a vehicle is connected, disconnect it // If a vehicle is connected, disconnect it
if (_activeVehicle) { if (_activeVehicle) {
UAS* uas = _activeVehicle->uas(); UAS* uas = _activeVehicle->uas();
disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed);
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);
} }
// Always set up the new vehicle // Always set up the new vehicle
_activeVehicle = vehicle; _activeVehicle = vehicle;
// If joystick is not calibrated, disable it // If joystick is not calibrated, disable it
if ( !_calibrated ) { if ( !_calibrated ) {
vehicle->setJoystickEnabled(false); vehicle->setJoystickEnabled(false);
} }
// Update qml in case of joystick transition // Update qml in case of joystick transition
emit calibratedChanged(_calibrated); emit calibratedChanged(_calibrated);
// Only connect the new vehicle if it wants joystick data // Only connect the new vehicle if it wants joystick data
if (vehicle->joystickEnabled()) { if (vehicle->joystickEnabled()) {
_pollingStartedForCalibration = false; _pollingStartedForCalibration = false;
UAS* uas = _activeVehicle->uas(); UAS* uas = _activeVehicle->uas();
connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); connect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed);
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);
// FIXME: **** // FIXME: ****
//connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); //connect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
} }
} }
if (!isRunning()) { if (!isRunning()) {
_exitThread = false; _exitThread = false;
start(); start();
...@@ -589,16 +610,20 @@ void Joystick::startPolling(Vehicle* vehicle) ...@@ -589,16 +610,20 @@ void Joystick::startPolling(Vehicle* vehicle)
void Joystick::stopPolling(void) void Joystick::stopPolling(void)
{ {
if (isRunning()) { if (isRunning()) {
if (_activeVehicle && _activeVehicle->joystickEnabled()) { if (_activeVehicle && _activeVehicle->joystickEnabled()) {
UAS* uas = _activeVehicle->uas(); UAS* uas = _activeVehicle->uas();
// Neutral attitude controls // Neutral attitude controls
// emit manualControl(0, 0, 0, 0.5, 0, _activeVehicle->joystickMode()); // emit manualControl(0, 0, 0, 0.5, 0, _activeVehicle->joystickMode());
disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint); disconnect(this, &Joystick::manualControl, uas, &UAS::setExternalControlSetpoint);
disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmed);
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);
} }
// FIXME: **** // FIXME: ****
//disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction); //disconnect(this, &Joystick::buttonActionTriggered, uas, &UAS::triggerAction);
_exitThread = true; _exitThread = true;
} }
} }
...@@ -609,7 +634,6 @@ void Joystick::setCalibration(int axis, Calibration_t& calibration) ...@@ -609,7 +634,6 @@ void Joystick::setCalibration(int axis, Calibration_t& calibration)
qCWarning(JoystickLog) << "Invalid axis index" << axis; qCWarning(JoystickLog) << "Invalid axis index" << axis;
return; return;
} }
_calibrated = true; _calibrated = true;
_rgCalibration[axis] = calibration; _rgCalibration[axis] = calibration;
_saveSettings(); _saveSettings();
...@@ -631,7 +655,6 @@ void Joystick::setFunctionAxis(AxisFunction_t function, int axis) ...@@ -631,7 +655,6 @@ void Joystick::setFunctionAxis(AxisFunction_t function, int axis)
qCWarning(JoystickLog) << "Invalid axis index" << axis; qCWarning(JoystickLog) << "Invalid axis index" << axis;
return; return;
} }
_calibrated = true; _calibrated = true;
_rgFunctionAxis[function] = axis; _rgFunctionAxis[function] = axis;
_saveSettings(); _saveSettings();
...@@ -646,7 +669,7 @@ int Joystick::getFunctionAxis(AxisFunction_t function) ...@@ -646,7 +669,7 @@ int Joystick::getFunctionAxis(AxisFunction_t function)
return _rgFunctionAxis[function]; return _rgFunctionAxis[function];
} }
QStringList Joystick::actions(void) QStringList Joystick::actions()
{ {
QStringList list; QStringList list;
list << _buttonActionArm << _buttonActionDisarm << _buttonActionToggleArm; list << _buttonActionArm << _buttonActionDisarm << _buttonActionToggleArm;
...@@ -692,7 +715,7 @@ QString Joystick::getButtonAction(int button) ...@@ -692,7 +715,7 @@ QString Joystick::getButtonAction(int button)
return _rgButtonActions[button]; return _rgButtonActions[button];
} }
QVariantList Joystick::buttonActions(void) QVariantList Joystick::buttonActions()
{ {
QVariantList list; QVariantList list;
for (int button=0; button<_totalButtonCount; button++) { for (int button=0; button<_totalButtonCount; button++) {
...@@ -787,6 +810,13 @@ float Joystick::frequency() ...@@ -787,6 +810,13 @@ float Joystick::frequency()
return _frequency; return _frequency;
} }
void Joystick::setGimbalEnabled(bool set)
{
_gimbalEnabled = set;
_saveSettings();
emit gimbalEnabledChanged();
}
void Joystick::setFrequency(float val) void Joystick::setFrequency(float val)
{ {
//-- Arbitrary limits //-- Arbitrary limits
...@@ -816,17 +846,17 @@ void Joystick::_buttonAction(const QString& action) ...@@ -816,17 +846,17 @@ void Joystick::_buttonAction(const QString& action)
return; return;
} }
if (action == _buttonActionArm) { if (action == _buttonActionArm) {
_activeVehicle->setArmed(true); emit setArmed(true);
} else if (action == _buttonActionDisarm) { } else if (action == _buttonActionDisarm) {
_activeVehicle->setArmed(false); emit setArmed(false);
} else if (action == _buttonActionToggleArm) { } else if (action == _buttonActionToggleArm) {
_activeVehicle->setArmed(!_activeVehicle->armed()); emit setArmed(!_activeVehicle->armed());
} else if (action == _buttonActionVTOLFixedWing) { } else if (action == _buttonActionVTOLFixedWing) {
_activeVehicle->setVtolInFwdFlight(true); emit setVtolInFwdFlight(true);
} else if (action == _buttonActionVTOLMultiRotor) { } else if (action == _buttonActionVTOLMultiRotor) {
_activeVehicle->setVtolInFwdFlight(false); emit setVtolInFwdFlight(false);
} else if (_activeVehicle->flightModes().contains(action)) { } else if (_activeVehicle->flightModes().contains(action)) {
_activeVehicle->setFlightMode(action); emit setFlightMode(action);
} else if(action == _buttonActionZoomIn || action == _buttonActionZoomOut) { } else if(action == _buttonActionZoomIn || action == _buttonActionZoomOut) {
emit stepZoom(action == _buttonActionZoomIn ? 1 : -1); emit stepZoom(action == _buttonActionZoomIn ? 1 : -1);
} else if(action == _buttonActionNextStream || action == _buttonActionPreviousStream) { } else if(action == _buttonActionNextStream || action == _buttonActionPreviousStream) {
...@@ -842,15 +872,15 @@ void Joystick::_buttonAction(const QString& action) ...@@ -842,15 +872,15 @@ void Joystick::_buttonAction(const QString& action)
} else if(action == _buttonActionToggleVideoRecord) { } else if(action == _buttonActionToggleVideoRecord) {
emit toggleVideoRecord(); emit toggleVideoRecord();
} else if(action == _buttonActionGimbalUp) { } else if(action == _buttonActionGimbalUp) {
_activeVehicle->gimbalPitchStep(1); emit gimbalPitchStep(1);
} else if(action == _buttonActionGimbalDown) { } else if(action == _buttonActionGimbalDown) {
_activeVehicle->gimbalPitchStep(-1); emit gimbalPitchStep(-1);
} else if(action == _buttonActionGimbalLeft) { } else if(action == _buttonActionGimbalLeft) {
_activeVehicle->gimbalYawStep(-1); emit gimbalYawStep(-1);
} else if(action == _buttonActionGimbalRight) { } else if(action == _buttonActionGimbalRight) {
_activeVehicle->gimbalYawStep(1); emit gimbalYawStep(1);
} else if(action == _buttonActionGimbalCenter) { } else if(action == _buttonActionGimbalCenter) {
_activeVehicle->centerGimbal(); emit centerGimbal();
} else { } else {
qCDebug(JoystickLog) << "_buttonAction unknown action:" << action; qCDebug(JoystickLog) << "_buttonAction unknown action:" << action;
} }
......
src/Joystick/Joystick.h
View file @ 3ee54e56
...@@ -60,31 +60,35 @@ public: ...@@ -60,31 +60,35 @@ public:
ThrottleModeMax ThrottleModeMax
} ThrottleMode_t; } ThrottleMode_t;
Q_PROPERTY(QString name READ name CONSTANT) Q_PROPERTY(QString name READ name CONSTANT)
Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged)
Q_PROPERTY(bool calibrated MEMBER _calibrated NOTIFY calibratedChanged) Q_PROPERTY(int totalButtonCount READ totalButtonCount CONSTANT)
Q_PROPERTY(int axisCount READ axisCount CONSTANT)
Q_PROPERTY(int totalButtonCount READ totalButtonCount CONSTANT) Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT)
Q_PROPERTY(int axisCount READ axisCount CONSTANT) Q_PROPERTY(QStringList actions READ actions CONSTANT)
Q_PROPERTY(QVariantList buttonActions READ buttonActions NOTIFY buttonActionsChanged)
Q_PROPERTY(QStringList actions READ actions CONSTANT)
Q_PROPERTY(bool gimbalEnabled READ gimbalEnabled WRITE setGimbalEnabled NOTIFY gimbalEnabledChanged)
Q_PROPERTY(QVariantList buttonActions READ buttonActions NOTIFY buttonActionsChanged) Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged)
Q_INVOKABLE void setButtonAction(int button, const QString& action); Q_PROPERTY(float frequency READ frequency WRITE setFrequency NOTIFY frequencyChanged)
Q_INVOKABLE QString getButtonAction(int button); Q_PROPERTY(bool negativeThrust READ negativeThrust WRITE setNegativeThrust NOTIFY negativeThrustChanged)
Q_PROPERTY(float exponential READ exponential WRITE setExponential NOTIFY exponentialChanged)
Q_PROPERTY(int throttleMode READ throttleMode WRITE setThrottleMode NOTIFY throttleModeChanged) Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged)
Q_PROPERTY(bool negativeThrust READ negativeThrust WRITE setNegativeThrust NOTIFY negativeThrustChanged) Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged)
Q_PROPERTY(float exponential READ exponential WRITE setExponential NOTIFY exponentialChanged)
Q_PROPERTY(bool accumulator READ accumulator WRITE setAccumulator NOTIFY accumulatorChanged) Q_INVOKABLE void setButtonAction (int button, const QString& action);
Q_PROPERTY(bool requiresCalibration READ requiresCalibration CONSTANT) Q_INVOKABLE QString getButtonAction (int button);
Q_PROPERTY(bool circleCorrection READ circleCorrection WRITE setCircleCorrection NOTIFY circleCorrectionChanged)
Q_PROPERTY(float frequency READ frequency WRITE setFrequency NOTIFY frequencyChanged)
// Property accessors // Property accessors
int axisCount(void) { return _axisCount; } QString name () { return _name; }
int totalButtonCount(void) { return _totalButtonCount; } int totalButtonCount () { return _totalButtonCount; }
int axisCount () { return _axisCount; }
bool gimbalEnabled () { return _gimbalEnabled; }
QStringList actions ();
QVariantList buttonActions ();
void setGimbalEnabled (bool set);
/// Start the polling thread which will in turn emit joystick signals /// Start the polling thread which will in turn emit joystick signals
void startPolling(Vehicle* vehicle); void startPolling(Vehicle* vehicle);
...@@ -96,10 +100,7 @@ public: ...@@ -96,10 +100,7 @@ public:
void setFunctionAxis(AxisFunction_t function, int axis); void setFunctionAxis(AxisFunction_t function, int axis);
int getFunctionAxis(AxisFunction_t function); int getFunctionAxis(AxisFunction_t function);
QStringList actions(void);
QVariantList buttonActions(void);
QString name(void) { return _name; }
/* /*
// Joystick index used by sdl library // Joystick index used by sdl library
// Settable because sdl library remaps indices after certain events // Settable because sdl library remaps indices after certain events
...@@ -162,18 +163,26 @@ signals: ...@@ -162,18 +163,26 @@ signals:
/// @param yaw Range is -1:1, negative meaning yaw left, positive meaning yaw right /// @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 throttle Range is 0:1, 0 meaning no throttle, 1 meaning full throttle
/// @param mode See Vehicle::JoystickMode_t enum /// @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, quint16 buttons, int joystickMmode);
void manualControlGimbal (float gimbalPitch, float gimbalYaw);
void buttonActionTriggered(int action); void buttonActionTriggered(int action);
void frequencyChanged (); void gimbalEnabledChanged ();
void stepZoom (int direction); void frequencyChanged ();
void stepCamera (int direction); void stepZoom (int direction);
void stepStream (int direction); void stepCamera (int direction);
void triggerCamera (); void stepStream (int direction);
void startVideoRecord (); void triggerCamera ();
void stopVideoRecord (); void startVideoRecord ();
void toggleVideoRecord (); void stopVideoRecord ();
void toggleVideoRecord ();
void gimbalPitchStep (int direction);
void gimbalYawStep (int direction);
void centerGimbal ();
void setArmed (bool arm);
void setVtolInFwdFlight (bool set);
void setFlightMode (const QString& flightMode);
protected: protected:
void _setDefaultCalibration (); void _setDefaultCalibration ();
...@@ -185,29 +194,35 @@ protected: ...@@ -185,29 +194,35 @@ protected:
bool _validButton (int button); bool _validButton (int button);
private: private:
virtual bool _open() = 0; virtual bool _open () = 0;
virtual void _close() = 0; virtual void _close () = 0;
virtual bool _update() = 0; virtual bool _update () = 0;
virtual bool _getButton(int i) = 0; virtual bool _getButton (int i) = 0;
virtual int _getAxis(int i) = 0; virtual int _getAxis (int i) = 0;
virtual uint8_t _getHat(int hat,int i) = 0; virtual bool _getHat (int hat,int i) = 0;
void _updateTXModeSettingsKey(Vehicle* activeVehicle); void _updateTXModeSettingsKey(Vehicle* activeVehicle);
int _mapFunctionMode(int mode, int function); int _mapFunctionMode(int mode, int function);
void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]); void _remapAxes(int currentMode, int newMode, int (&newMapping)[maxFunction]);
// Override from QThread // Override from QThread
virtual void run(void); virtual void run();
protected: protected:
enum {
BUTTON_UP,
BUTTON_DOWN,
BUTTON_REPEAT
};
uint8_t*_rgButtonValues = nullptr;
bool _exitThread = false; ///< true: signal thread to exit bool _exitThread = false; ///< true: signal thread to exit
bool _calibrationMode = false; bool _calibrationMode = false;
int* _rgAxisValues = nullptr; int* _rgAxisValues = nullptr;
Calibration_t* _rgCalibration = nullptr; Calibration_t* _rgCalibration = nullptr;
bool* _rgButtonValues = nullptr;
quint16 _lastButtonBits = 0;
ThrottleMode_t _throttleMode = ThrottleModeDownZero; ThrottleMode_t _throttleMode = ThrottleModeDownZero;
bool _negativeThrust = false; bool _negativeThrust = false;
float _exponential = 0; float _exponential = 0;
...@@ -216,7 +231,7 @@ protected: ...@@ -216,7 +231,7 @@ protected:
bool _circleCorrection = true; bool _circleCorrection = true;
float _frequency = 25.0f; float _frequency = 25.0f;
Vehicle* _activeVehicle = nullptr; Vehicle* _activeVehicle = nullptr;
bool _gimbalEnabled = false;
bool _pollingStartedForCalibration = false; bool _pollingStartedForCalibration = false;
...@@ -234,7 +249,7 @@ protected: ...@@ -234,7 +249,7 @@ protected:
QStringList _rgButtonActions; QStringList _rgButtonActions;
MultiVehicleManager* _multiVehicleManager; MultiVehicleManager* _multiVehicleManager = nullptr;
private: private:
static const char* _rgFunctionSettingsKey[maxFunction]; static const char* _rgFunctionSettingsKey[maxFunction];
...@@ -254,6 +269,7 @@ private: ...@@ -254,6 +269,7 @@ private:
static const char* _roverTXModeSettingsKey; static const char* _roverTXModeSettingsKey;
static const char* _vtolTXModeSettingsKey; static const char* _vtolTXModeSettingsKey;
static const char* _submarineTXModeSettingsKey; static const char* _submarineTXModeSettingsKey;
static const char* _gimbalSettingsKey;
static const char* _buttonActionArm; static const char* _buttonActionArm;
static const char* _buttonActionDisarm; static const char* _buttonActionDisarm;
......
src/Joystick/JoystickAndroid.cc
View file @ 3ee54e56
...@@ -200,11 +200,10 @@ int JoystickAndroid::_getAxis(int i) { ...@@ -200,11 +200,10 @@ int JoystickAndroid::_getAxis(int i) {
return axisValue[ i ]; return axisValue[ i ];
} }
uint8_t JoystickAndroid::_getHat(int hat,int i) { bool JoystickAndroid::_getHat(int hat,int i) {
Q_UNUSED(hat); Q_UNUSED(hat);
Q_UNUSED(i); Q_UNUSED(i);
return false;
return 0;
} }
static JoystickManager *_manager = nullptr; static JoystickManager *_manager = nullptr;
......
src/Joystick/JoystickAndroid.h
View file @ 3ee54e56
...@@ -29,13 +29,13 @@ private: ...@@ -29,13 +29,13 @@ private:
bool handleKeyEvent(jobject event); bool handleKeyEvent(jobject event);
bool handleGenericMotionEvent(jobject event); bool handleGenericMotionEvent(jobject event);
virtual bool _open(); virtual bool _open ();
virtual void _close(); virtual void _close ();
virtual bool _update(); virtual bool _update ();
virtual bool _getButton(int i); virtual bool _getButton (int i);
virtual int _getAxis(int i); virtual int _getAxis (int i);
virtual uint8_t _getHat(int hat,int i); virtual bool _getHat (int hat,int i);
int *btnCode; int *btnCode;
int *axisCode; int *axisCode;
......
src/Joystick/JoystickSDL.cc
View file @ 3ee54e56
...@@ -19,7 +19,6 @@ bool JoystickSDL::init(void) { ...@@ -19,7 +19,6 @@ bool JoystickSDL::init(void) {
qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError(); qWarning() << "Couldn't initialize SimpleDirectMediaLayer:" << SDL_GetError();
return false; return false;
} }
_loadGameControllerMappings(); _loadGameControllerMappings();
return true; return true;
} }
...@@ -78,7 +77,7 @@ QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicl ...@@ -78,7 +77,7 @@ QMap<QString, Joystick*> JoystickSDL::discover(MultiVehicleManager* _multiVehicl
newRet[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager); newRet[name] = new JoystickSDL(name, qMax(0,axisCount), qMax(0,buttonCount), qMax(0,hatCount), i, isGameController, _multiVehicleManager);
} else { } else {
newRet[name] = ret[name]; newRet[name] = ret[name];
JoystickSDL *j = (JoystickSDL*)newRet[name]; JoystickSDL *j = static_cast<JoystickSDL*>(newRet[name]);
if (j->index() != i) { if (j->index() != i) {
j->setIndex(i); // This joystick index has been remapped by SDL j->setIndex(i); // This joystick index has been remapped by SDL
} }
...@@ -131,7 +130,7 @@ bool JoystickSDL::_open(void) { ...@@ -131,7 +130,7 @@ bool JoystickSDL::_open(void) {
} }
void JoystickSDL::_close(void) { void JoystickSDL::_close(void) {
if (sdlJoystick == NULL) { if (sdlJoystick == nullptr) {
qCDebug(JoystickLog) << "Attempt to close null joystick!"; qCDebug(JoystickLog) << "Attempt to close null joystick!";
return; return;
} }
...@@ -152,8 +151,8 @@ void JoystickSDL::_close(void) { ...@@ -152,8 +151,8 @@ void JoystickSDL::_close(void) {
} }
} }
sdlJoystick = NULL; sdlJoystick = nullptr;
sdlController = NULL; sdlController = nullptr;
} }
bool JoystickSDL::_update(void) bool JoystickSDL::_update(void)
...@@ -164,26 +163,25 @@ bool JoystickSDL::_update(void) ...@@ -164,26 +163,25 @@ bool JoystickSDL::_update(void)
} }
bool JoystickSDL::_getButton(int i) { bool JoystickSDL::_getButton(int i) {
if ( _isGameController ) { if (_isGameController) {
return !!SDL_GameControllerGetButton(sdlController, SDL_GameControllerButton(i)); return SDL_GameControllerGetButton(sdlController, SDL_GameControllerButton(i)) == 1;
} else { } else {
return !!SDL_JoystickGetButton(sdlJoystick, i); return SDL_JoystickGetButton(sdlJoystick, i) == 1;
} }
} }
int JoystickSDL::_getAxis(int i) { int JoystickSDL::_getAxis(int i) {
if ( _isGameController ) { if (_isGameController) {
return SDL_GameControllerGetAxis(sdlController, SDL_GameControllerAxis(i)); return SDL_GameControllerGetAxis(sdlController, SDL_GameControllerAxis(i));
} else { } else {
return SDL_JoystickGetAxis(sdlJoystick, i); return SDL_JoystickGetAxis(sdlJoystick, i);
} }
} }
uint8_t JoystickSDL::_getHat(int hat,int i) { bool JoystickSDL::_getHat(int hat, int i) {
uint8_t hatButtons[] = {SDL_HAT_UP,SDL_HAT_DOWN,SDL_HAT_LEFT,SDL_HAT_RIGHT}; uint8_t hatButtons[] = {SDL_HAT_UP,SDL_HAT_DOWN,SDL_HAT_LEFT,SDL_HAT_RIGHT};
if (i < int(sizeof(hatButtons))) {
if ( i < int(sizeof(hatButtons)) ) { return (SDL_JoystickGetHat(sdlJoystick, hat) & hatButtons[i]) != 0;
return !!(SDL_JoystickGetHat(sdlJoystick, hat) & hatButtons[i]);
} }
return 0; return false;
} }
src/Joystick/JoystickSDL.h
View file @ 3ee54e56
...@@ -24,16 +24,17 @@ public: ...@@ -24,16 +24,17 @@ public:
private: private:
static void _loadGameControllerMappings(); static void _loadGameControllerMappings();
bool _open() final; bool _open () final;
void _close() final; void _close () final;
bool _update() final; bool _update () final;
bool _getButton(int i) final; bool _getButton (int i) final;
int _getAxis(int i) final; int _getAxis (int i) final;
uint8_t _getHat(int hat,int i) final; bool _getHat (int hat,int i) final;
SDL_Joystick* sdlJoystick;
SDL_GameController* sdlController;
SDL_Joystick *sdlJoystick;
SDL_GameController *sdlController;
bool _isGameController; bool _isGameController;
int _index; ///< Index for SDL_JoystickOpen int _index; ///< Index for SDL_JoystickOpen
......
src/Vehicle/Vehicle.cc
View file @ 3ee54e56
...@@ -3064,26 +3064,29 @@ void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, ...@@ -3064,26 +3064,29 @@ void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius,
qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle.")); qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle."));
return; return;
} }
if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) { if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) {
sendMavCommandInt(defaultComponentId(), sendMavCommandInt(
MAV_CMD_DO_ORBIT, defaultComponentId(),
MAV_FRAME_GLOBAL, MAV_CMD_DO_ORBIT,
true, // show error if fails MAV_FRAME_GLOBAL,
radius, true, // show error if fails
qQNaN(), // Use default velocity static_cast<float>(radius),
0, // Vehicle points to center static_cast<float>(qQNaN()), // Use default velocity
qQNaN(), // reserved 0, // Vehicle points to center
centerCoord.latitude(), centerCoord.longitude(), amslAltitude); static_cast<float>(qQNaN()), // reserved
centerCoord.latitude(), centerCoord.longitude(), static_cast<float>(amslAltitude));
} else { } else {
sendMavCommand(defaultComponentId(), sendMavCommand(
MAV_CMD_DO_ORBIT, defaultComponentId(),
true, // show error if fails MAV_CMD_DO_ORBIT,
radius, true, // show error if fails
qQNaN(), // Use default velocity static_cast<float>(radius),
0, // Vehicle points to center static_cast<float>(qQNaN()), // Use default velocity
qQNaN(), // reserved 0, // Vehicle points to center
centerCoord.latitude(), centerCoord.longitude(), amslAltitude); static_cast<float>(qQNaN()), // reserved
static_cast<float>(centerCoord.latitude()),
static_cast<float>(centerCoord.longitude()),
static_cast<float>(amslAltitude));
} }
} }
...@@ -3098,10 +3101,11 @@ void Vehicle::pauseVehicle(void) ...@@ -3098,10 +3101,11 @@ void Vehicle::pauseVehicle(void)
void Vehicle::abortLanding(double climbOutAltitude) void Vehicle::abortLanding(double climbOutAltitude)
{ {
sendMavCommand(defaultComponentId(), sendMavCommand(
MAV_CMD_DO_GO_AROUND, defaultComponentId(),
true, // show error if fails MAV_CMD_DO_GO_AROUND,
climbOutAltitude); true, // show error if fails
static_cast<float>(climbOutAltitude));
} }
bool Vehicle::guidedMode(void) const bool Vehicle::guidedMode(void) const
...@@ -3116,11 +3120,12 @@ void Vehicle::setGuidedMode(bool guidedMode) ...@@ -3116,11 +3120,12 @@ void Vehicle::setGuidedMode(bool guidedMode)
void Vehicle::emergencyStop(void) void Vehicle::emergencyStop(void)
{ {
sendMavCommand(_defaultComponentId, sendMavCommand(
MAV_CMD_COMPONENT_ARM_DISARM, _defaultComponentId,
true, // show error if fails MAV_CMD_COMPONENT_ARM_DISARM,
0.0f, true, // show error if fails
21196.0f); // Magic number for emergency stop 0.0f,
21196.0f); // Magic number for emergency stop
} }
void Vehicle::setCurrentMissionSequence(int seq) void Vehicle::setCurrentMissionSequence(int seq)
...@@ -3129,13 +3134,14 @@ void Vehicle::setCurrentMissionSequence(int seq) ...@@ -3129,13 +3134,14 @@ void Vehicle::setCurrentMissionSequence(int seq)
seq--; seq--;
} }
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_mission_set_current_pack_chan(_mavlink->getSystemId(), mavlink_msg_mission_set_current_pack_chan(
_mavlink->getComponentId(), static_cast<uint8_t>(_mavlink->getSystemId()),
priorityLink()->mavlinkChannel(), static_cast<uint8_t>(_mavlink->getComponentId()),
&msg, priorityLink()->mavlinkChannel(),
id(), &msg,
_compID, static_cast<uint8_t>(id()),
seq); _compID,
static_cast<uint8_t>(seq));
sendMessageOnLink(priorityLink(), msg); sendMessageOnLink(priorityLink(), msg);
} }
...@@ -3147,13 +3153,13 @@ void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, flo ...@@ -3147,13 +3153,13 @@ void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, flo
entry.component = component; entry.component = component;
entry.command = command; entry.command = command;
entry.showError = showError; entry.showError = showError;
entry.rgParam[0] = param1; entry.rgParam[0] = static_cast<double>(param1);
entry.rgParam[1] = param2; entry.rgParam[1] = static_cast<double>(param2);
entry.rgParam[2] = param3; entry.rgParam[2] = static_cast<double>(param3);
entry.rgParam[3] = param4; entry.rgParam[3] = static_cast<double>(param4);
entry.rgParam[4] = param5; entry.rgParam[4] = static_cast<double>(param5);
entry.rgParam[5] = param6; entry.rgParam[5] = static_cast<double>(param6);
entry.rgParam[6] = param7; entry.rgParam[6] = static_cast<double>(param7);
_mavCommandQueue.append(entry); _mavCommandQueue.append(entry);
...@@ -3172,13 +3178,13 @@ void Vehicle::sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame, ...@@ -3172,13 +3178,13 @@ void Vehicle::sendMavCommandInt(int component, MAV_CMD command, MAV_FRAME frame,
entry.command = command; entry.command = command;
entry.frame = frame; entry.frame = frame;
entry.showError = showError; entry.showError = showError;
entry.rgParam[0] = param1; entry.rgParam[0] = static_cast<double>(param1);
entry.rgParam[1] = param2; entry.rgParam[1] = static_cast<double>(param2);
entry.rgParam[2] = param3; entry.rgParam[2] = static_cast<double>(param3);
entry.rgParam[3] = param4; entry.rgParam[3] = static_cast<double>(param4);
entry.rgParam[4] = param5; entry.rgParam[4] = param5;
entry.rgParam[5] = param6; entry.rgParam[5] = param6;
entry.rgParam[6] = param7; entry.rgParam[6] = static_cast<double>(param7);
_mavCommandQueue.append(entry); _mavCommandQueue.append(entry);
...@@ -3367,8 +3373,7 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) ...@@ -3367,8 +3373,7 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */); emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
if (showError) { if (showError) {
QString commandName = _toolbox->missionCommandTree()->friendlyName((MAV_CMD)ack.command); QString commandName = _toolbox->missionCommandTree()->friendlyName(static_cast<MAV_CMD>(ack.command));
switch (ack.result) { switch (ack.result) {
case MAV_RESULT_TEMPORARILY_REJECTED: case MAV_RESULT_TEMPORARILY_REJECTED:
qgcApp()->showMessage(tr("%1 command temporarily rejected").arg(commandName)); qgcApp()->showMessage(tr("%1 command temporarily rejected").arg(commandName));
...@@ -4016,6 +4021,7 @@ void Vehicle::gimbalPitchStep(int direction) ...@@ -4016,6 +4021,7 @@ void Vehicle::gimbalPitchStep(int direction)
void Vehicle::gimbalYawStep(int direction) void Vehicle::gimbalYawStep(int direction)
{ {
if(!_haveGimbalData) { if(!_haveGimbalData) {
qDebug() << "Yaw:" << _curGinmbalYaw << direction << (_curGinmbalYaw + direction);
double y = static_cast<double>(_curGinmbalYaw + direction); double y = static_cast<double>(_curGinmbalYaw + direction);
gimbalControlValue(static_cast<double>(_curGinmbalYaw), y); gimbalControlValue(static_cast<double>(_curGinmbalYaw), y);
} }
......
src/Vehicle/Vehicle.h
View file @ 3ee54e56
...@@ -840,8 +840,8 @@ public: ...@@ -840,8 +840,8 @@ public:
QGeoCoordinate homePosition(void); QGeoCoordinate homePosition(void);
bool armed(void) { return _armed; } bool armed () { return _armed; }
void setArmed(bool armed); void setArmed (bool armed);
bool flightModeSetAvailable(void); bool flightModeSetAvailable(void);
QStringList flightModes(void); QStringList flightModes(void);
...@@ -1084,7 +1084,6 @@ public: ...@@ -1084,7 +1084,6 @@ public:
void _setFlying(bool flying); void _setFlying(bool flying);
void _setLanding(bool landing); void _setLanding(bool landing);
void setVtolInFwdFlight(bool vtolInFwdFlight);
void _setHomePosition(QGeoCoordinate& homeCoord); void _setHomePosition(QGeoCoordinate& homeCoord);
void _setMaxProtoVersion (unsigned version); void _setMaxProtoVersion (unsigned version);
...@@ -1101,6 +1100,9 @@ public: ...@@ -1101,6 +1100,9 @@ public:
qreal gimbalYaw () { return static_cast<qreal>(_curGinmbalYaw); } qreal gimbalYaw () { return static_cast<qreal>(_curGinmbalYaw); }
bool gimbalData () { return _haveGimbalData; } bool gimbalData () { return _haveGimbalData; }
public slots:
void setVtolInFwdFlight (bool vtolInFwdFlight);
signals: signals:
void allLinksInactive(Vehicle* vehicle); void allLinksInactive(Vehicle* vehicle);
void coordinateChanged(QGeoCoordinate coordinate); void coordinateChanged(QGeoCoordinate coordinate);
......
src/VehicleSetup/JoystickConfigAdvanced.qml
View file @ 3ee54e56
...@@ -103,6 +103,30 @@ Item { ...@@ -103,6 +103,30 @@ Item {
} }
} }
} }
//---------------------------------------------------------------------
//-- Enable Gimbal
QGCLabel {
text: qsTr("Enable gimbal control (Experimental)")
visible: advancedSettings.checked
Layout.alignment: Qt.AlignVCenter
}
QGCCheckBox {
id: enabledGimbal
visible: advancedSettings.checked
enabled: _activeJoystick
onClicked: _activeJoystick.gimbalEnabled = checked
Component.onCompleted: {
checked = _activeJoystick.gimbalEnabled
}
Connections {
target: joystickManager
onActiveJoystickChanged: {
if(_activeJoystick) {
enabledGimbal.checked = Qt.binding(function() { return _activeJoystick.gimbalEnabled })
}
}
}
}
//----------------------------------------------------------------- //-----------------------------------------------------------------
//-- Mode //-- Mode
QGCLabel { QGCLabel {
......
src/VehicleSetup/JoystickConfigCalibration.qml
View file @ 3ee54e56
...@@ -96,7 +96,7 @@ Item { ...@@ -96,7 +96,7 @@ Item {
width: ScreenTools.defaultFontPixelWidth * 0.125 width: ScreenTools.defaultFontPixelWidth * 0.125
height: parent.height * 0.2 height: parent.height * 0.2
color: qgcPal.text color: qgcPal.text
visible: controller.hasGimbal visible: controller.hasGimbalPitch && _activeJoystick.gimbalEnabled
x: (parent.width * 0.5) - (width * 0.5) x: (parent.width * 0.5) - (width * 0.5)
y: (parent.height * 0.5) - (height * 0.5) y: (parent.height * 0.5) - (height * 0.5)
} }
...@@ -105,7 +105,7 @@ Item { ...@@ -105,7 +105,7 @@ Item {
width: parent.width * 0.035 width: parent.width * 0.035
height: width height: width
radius: width * 0.5 radius: width * 0.5
visible: controller.hasGimbal visible: controller.hasGimbalPitch && _activeJoystick.gimbalEnabled
x: (parent.width * controller.gimbalPositions[0]) - (width * 0.5) x: (parent.width * controller.gimbalPositions[0]) - (width * 0.5)
y: (parent.height * controller.gimbalPositions[1]) - (height * 0.5) y: (parent.height * controller.gimbalPositions[1]) - (height * 0.5)
} }
...@@ -115,7 +115,7 @@ Item { ...@@ -115,7 +115,7 @@ Item {
width: parent.width * 0.2 width: parent.width * 0.2
height: ScreenTools.defaultFontPixelWidth * 0.125 height: ScreenTools.defaultFontPixelWidth * 0.125
color: qgcPal.text color: qgcPal.text
visible: controller.hasGimbal visible: controller.hasGimbalYaw && _activeJoystick.gimbalEnabled
x: (parent.width * 0.5) - (width * 0.5) x: (parent.width * 0.5) - (width * 0.5)
y: (parent.height * 0.3) - (height * 0.5) y: (parent.height * 0.3) - (height * 0.5)
} }
...@@ -124,7 +124,7 @@ Item { ...@@ -124,7 +124,7 @@ Item {
width: parent.width * 0.035 width: parent.width * 0.035
height: width height: width
radius: width * 0.5 radius: width * 0.5
visible: controller.hasGimbal visible: controller.hasGimbalYaw && _activeJoystick.gimbalEnabled
x: (parent.width * controller.gimbalPositions[2]) - (width * 0.5) x: (parent.width * controller.gimbalPositions[2]) - (width * 0.5)
y: (parent.height * controller.gimbalPositions[3]) - (height * 0.5) y: (parent.height * controller.gimbalPositions[3]) - (height * 0.5)
} }
......
src/VehicleSetup/JoystickConfigController.cc
View file @ 3ee54e56
...@@ -12,8 +12,6 @@ ...@@ -12,8 +12,6 @@
#include "JoystickManager.h" #include "JoystickManager.h"
#include "QGCApplication.h" #include "QGCApplication.h"
#include <QSettings>
QGC_LOGGING_CATEGORY(JoystickConfigControllerLog, "JoystickConfigControllerLog") QGC_LOGGING_CATEGORY(JoystickConfigControllerLog, "JoystickConfigControllerLog")
const int JoystickConfigController::_calCenterPoint = 0; const int JoystickConfigController::_calCenterPoint = 0;
...@@ -167,9 +165,14 @@ void JoystickConfigController::_advanceState() ...@@ -167,9 +165,14 @@ void JoystickConfigController::_advanceState()
{ {
_currentStep++; _currentStep++;
const stateMachineEntry* state = _getStateMachineEntry(_currentStep); const stateMachineEntry* state = _getStateMachineEntry(_currentStep);
while (state->channelID > _axisCount) { //-- Handle Gimbal
_currentStep++; if (state->channelID > _axisCount) {
state = _getStateMachineEntry(_currentStep); //-- No channels for gimbal
_advanceState();
}
if((state->channelID == 4 || state->channelID == 5) && !_activeJoystick->gimbalEnabled()) {
//-- Gimbal disabled. Skip it.
_advanceState();
} }
_setupCurrentState(); _setupCurrentState();
} }
...@@ -220,7 +223,10 @@ void JoystickConfigController::_axisValueChanged(int axis, int value) ...@@ -220,7 +223,10 @@ void JoystickConfigController::_axisValueChanged(int axis, int value)
// Track the axis count by keeping track of how many axes we see // Track the axis count by keeping track of how many axes we see
if (axis + 1 > static_cast<int>(_axisCount)) { if (axis + 1 > static_cast<int>(_axisCount)) {
_axisCount = axis + 1; _axisCount = axis + 1;
emit hasGimbalChanged(); if(_axisCount > 4)
emit hasGimbalPitchChanged();
if(_axisCount > 5)
emit hasGimbalYawChanged();
} }
} }
if (_currentStep != -1) { if (_currentStep != -1) {
...@@ -631,8 +637,8 @@ void JoystickConfigController::_setStickPositions() ...@@ -631,8 +637,8 @@ void JoystickConfigController::_setStickPositions()
_sticksYawRight = stLeftStickRight; _sticksYawRight = stLeftStickRight;
_sticksRollLeft = stRightStickLeft; _sticksRollLeft = stRightStickLeft;
_sticksRollRight = stRightStickRight; _sticksRollRight = stRightStickRight;
_sticksPitchUp = stLeftStickDown; _sticksPitchUp = stLeftStickUp;
_sticksPitchDown = stLeftStickUp; _sticksPitchDown = stLeftStickDown;
break; break;
case 2: case 2:
_sticksThrottleUp = stLeftStickUp; _sticksThrottleUp = stLeftStickUp;
...@@ -641,8 +647,8 @@ void JoystickConfigController::_setStickPositions() ...@@ -641,8 +647,8 @@ void JoystickConfigController::_setStickPositions()
_sticksYawRight = stLeftStickRight; _sticksYawRight = stLeftStickRight;
_sticksRollLeft = stRightStickLeft; _sticksRollLeft = stRightStickLeft;
_sticksRollRight = stRightStickRight; _sticksRollRight = stRightStickRight;
_sticksPitchUp = stRightStickDown; _sticksPitchUp = stRightStickUp;
_sticksPitchDown = stRightStickUp; _sticksPitchDown = stRightStickDown;
break; break;
case 3: case 3:
_sticksThrottleUp = stRightStickUp; _sticksThrottleUp = stRightStickUp;
...@@ -651,8 +657,8 @@ void JoystickConfigController::_setStickPositions() ...@@ -651,8 +657,8 @@ void JoystickConfigController::_setStickPositions()
_sticksYawRight = stRightStickRight; _sticksYawRight = stRightStickRight;
_sticksRollLeft = stLeftStickLeft; _sticksRollLeft = stLeftStickLeft;
_sticksRollRight = stLeftStickRight; _sticksRollRight = stLeftStickRight;
_sticksPitchUp = stLeftStickDown; _sticksPitchUp = stLeftStickUp;
_sticksPitchDown = stLeftStickUp; _sticksPitchDown = stLeftStickDown;
break; break;
case 4: case 4:
_sticksThrottleUp = stLeftStickUp; _sticksThrottleUp = stLeftStickUp;
...@@ -661,8 +667,8 @@ void JoystickConfigController::_setStickPositions() ...@@ -661,8 +667,8 @@ void JoystickConfigController::_setStickPositions()
_sticksYawRight = stRightStickRight; _sticksYawRight = stRightStickRight;
_sticksRollLeft = stLeftStickLeft; _sticksRollLeft = stLeftStickLeft;
_sticksRollRight = stLeftStickRight; _sticksRollRight = stLeftStickRight;
_sticksPitchUp = stRightStickDown; _sticksPitchUp = stRightStickUp;
_sticksPitchDown = stRightStickUp; _sticksPitchDown = stRightStickDown;
break; break;
default: default:
Q_ASSERT(false); Q_ASSERT(false);
...@@ -766,7 +772,8 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick) ...@@ -766,7 +772,8 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
delete[] _axisRawValue; delete[] _axisRawValue;
_axisCount = 0; _axisCount = 0;
_activeJoystick = nullptr; _activeJoystick = nullptr;
emit hasGimbalChanged(); emit hasGimbalPitchChanged();
emit hasGimbalYawChanged();
} }
if (joystick) { if (joystick) {
...@@ -781,7 +788,8 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick) ...@@ -781,7 +788,8 @@ void JoystickConfigController::_activeJoystickChanged(Joystick* joystick)
_axisRawValue = new int[_axisCount]; _axisRawValue = new int[_axisCount];
_setInternalCalibrationValuesFromSettings(); _setInternalCalibrationValuesFromSettings();
connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged); connect(_activeJoystick, &Joystick::rawAxisValueChanged, this, &JoystickConfigController::_axisValueChanged);
emit hasGimbalChanged(); emit hasGimbalPitchChanged();
emit hasGimbalYawChanged();
} }
} }
......
src/VehicleSetup/JoystickConfigController.h
View file @ 3ee54e56
...@@ -46,7 +46,9 @@ public: ...@@ -46,7 +46,9 @@ public:
Q_PROPERTY(bool yawAxisMapped READ yawAxisMapped NOTIFY yawAxisMappedChanged) Q_PROPERTY(bool yawAxisMapped READ yawAxisMapped NOTIFY yawAxisMappedChanged)
Q_PROPERTY(bool throttleAxisMapped READ throttleAxisMapped NOTIFY throttleAxisMappedChanged) Q_PROPERTY(bool throttleAxisMapped READ throttleAxisMapped NOTIFY throttleAxisMappedChanged)
Q_PROPERTY(bool hasGimbal READ hasGimbal NOTIFY hasGimbalChanged) Q_PROPERTY(bool hasGimbalPitch READ hasGimbalPitch NOTIFY hasGimbalPitchChanged)
Q_PROPERTY(bool hasGimbalYaw READ hasGimbalYaw NOTIFY hasGimbalYawChanged)
Q_PROPERTY(bool gimbalPitchAxisMapped READ gimbalPitchAxisMapped NOTIFY gimbalPitchAxisMappedChanged) Q_PROPERTY(bool gimbalPitchAxisMapped READ gimbalPitchAxisMapped NOTIFY gimbalPitchAxisMappedChanged)
Q_PROPERTY(bool gimbalYawAxisMapped READ gimbalYawAxisMapped NOTIFY gimbalYawAxisMappedChanged) Q_PROPERTY(bool gimbalYawAxisMapped READ gimbalYawAxisMapped NOTIFY gimbalYawAxisMappedChanged)
...@@ -90,7 +92,8 @@ public: ...@@ -90,7 +92,8 @@ public:
bool gimbalPitchAxisReversed (); bool gimbalPitchAxisReversed ();
bool gimbalYawAxisReversed (); bool gimbalYawAxisReversed ();
bool hasGimbal () { return _axisCount > 5; } bool hasGimbalPitch () { return _axisCount > 4; }
bool hasGimbalYaw () { return _axisCount > 5; }
bool getDeadbandToggle (); bool getDeadbandToggle ();
void setDeadbandToggle (bool); void setDeadbandToggle (bool);
...@@ -136,7 +139,8 @@ signals: ...@@ -136,7 +139,8 @@ signals:
void skipEnabledChanged (); void skipEnabledChanged ();
void stickPositionsChanged (); void stickPositionsChanged ();
void gimbalPositionsChanged (); void gimbalPositionsChanged ();
void hasGimbalChanged (); void hasGimbalPitchChanged ();
void hasGimbalYawChanged ();
void statusTextChanged (); void statusTextChanged ();
// @brief Signalled when in unit test mode and a message box should be displayed by the next button // @brief Signalled when in unit test mode and a message box should be displayed by the next button
......
src/VehicleSetup/JoystickConfigGeneral.qml
View file @ 3ee54e56
...@@ -149,6 +149,7 @@ Item { ...@@ -149,6 +149,7 @@ Item {
anchors.centerIn: parent anchors.centerIn: parent
QGCLabel { QGCLabel {
text: activeVehicle.sub ? qsTr("Lateral") : qsTr("Roll") text: activeVehicle.sub ? qsTr("Lateral") : qsTr("Roll")
Layout.minimumWidth: ScreenTools.defaultFontPixelWidth * 12
} }
AxisMonitor { AxisMonitor {
id: rollAxis id: rollAxis
...@@ -156,10 +157,6 @@ Item { ...@@ -156,10 +157,6 @@ Item {
width: axisMonitorWidth width: axisMonitorWidth
mapped: controller.rollAxisMapped mapped: controller.rollAxisMapped
reversed: controller.rollAxisReversed reversed: controller.rollAxisReversed
Connections {
target: _activeJoystick
onManualControl: rollAxis.axisValue = roll * 32768.0
}
} }
QGCLabel { QGCLabel {
...@@ -173,10 +170,6 @@ Item { ...@@ -173,10 +170,6 @@ Item {
width: axisMonitorWidth width: axisMonitorWidth
mapped: controller.pitchAxisMapped mapped: controller.pitchAxisMapped
reversed: controller.pitchAxisReversed reversed: controller.pitchAxisReversed
Connections {
target: _activeJoystick
onManualControl: pitchAxis.axisValue = pitch * 32768.0
}
} }
QGCLabel { QGCLabel {
...@@ -190,10 +183,6 @@ Item { ...@@ -190,10 +183,6 @@ Item {
width: axisMonitorWidth width: axisMonitorWidth
mapped: controller.yawAxisMapped mapped: controller.yawAxisMapped
reversed: controller.yawAxisReversed reversed: controller.yawAxisReversed
Connections {
target: _activeJoystick
onManualControl: yawAxis.axisValue = yaw * 32768.0
}
} }
QGCLabel { QGCLabel {
...@@ -207,9 +196,15 @@ Item { ...@@ -207,9 +196,15 @@ Item {
width: axisMonitorWidth width: axisMonitorWidth
mapped: controller.throttleAxisMapped mapped: controller.throttleAxisMapped
reversed: controller.throttleAxisReversed reversed: controller.throttleAxisReversed
Connections { }
target: _activeJoystick
onManualControl: throttleAxis.axisValue = _activeJoystick.negativeThrust ? throttle * -32768.0 : (-2 * throttle + 1) * 32768.0 Connections {
target: _activeJoystick
onManualControl: {
rollAxis.axisValue = roll * 32768.0
pitchAxis.axisValue = pitch * 32768.0
yawAxis.axisValue = yaw * 32768.0
throttleAxis.axisValue = _activeJoystick.negativeThrust ? throttle * -32768.0 : (-2 * throttle + 1) * 32768.0
} }
} }
...@@ -217,7 +212,7 @@ Item { ...@@ -217,7 +212,7 @@ Item {
id: gimbalPitchLabel id: gimbalPitchLabel
width: _attitudeLabelWidth width: _attitudeLabelWidth
text: qsTr("Gimbal Pitch") text: qsTr("Gimbal Pitch")
visible: controller.hasGimbal visible: controller.hasGimbalPitch && _activeJoystick.gimbalEnabled
} }
AxisMonitor { AxisMonitor {
id: gimbalPitchAxis id: gimbalPitchAxis
...@@ -225,18 +220,14 @@ Item { ...@@ -225,18 +220,14 @@ Item {
width: axisMonitorWidth width: axisMonitorWidth
mapped: controller.gimbalPitchAxisMapped mapped: controller.gimbalPitchAxisMapped
reversed: controller.gimbalPitchAxisReversed reversed: controller.gimbalPitchAxisReversed
visible: controller.hasGimbal visible: controller.hasGimbalPitch && _activeJoystick.gimbalEnabled
Connections {
target: _activeJoystick
onManualControl: gimbalPitchAxis.axisValue = gimbalPitch * 32768.0
}
} }
QGCLabel { QGCLabel {
id: gimbalYawLabel id: gimbalYawLabel
width: _attitudeLabelWidth width: _attitudeLabelWidth
text: qsTr("Gimbal Yaw") text: qsTr("Gimbal Yaw")
visible: controller.hasGimbal visible: controller.hasGimbalYaw && _activeJoystick.gimbalEnabled
} }
AxisMonitor { AxisMonitor {
id: gimbalYawAxis id: gimbalYawAxis
...@@ -244,9 +235,14 @@ Item { ...@@ -244,9 +235,14 @@ Item {
width: axisMonitorWidth width: axisMonitorWidth
mapped: controller.gimbalYawAxisMapped mapped: controller.gimbalYawAxisMapped
reversed: controller.gimbalYawAxisReversed reversed: controller.gimbalYawAxisReversed
Connections { visible: controller.hasGimbalYaw && _activeJoystick.gimbalEnabled
target: _activeJoystick }
onManualControl: gimbalYawAxis.axisValue = gimbalYaw * 32768.0
Connections {
target: _activeJoystick
onManualControlGimbal: {
gimbalPitchAxis.axisValue = gimbalPitch * 32768.0
gimbalYawAxis.axisValue = gimbalYaw * 32768.0
} }
} }
} }
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment