From 465c19c7da65b1d8f5263e78ca487c99cc508a52 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Jul 2014 10:48:21 +0200 Subject: [PATCH] joystick mode ui option this adds an option to select the mode of the joystick in the joystick config widget. The mode sets the setpoint type which is sent when moving the joystick. --- src/input/JoystickInput.cc | 2 ++ src/input/JoystickInput.h | 28 ++++++++++++++++++++++ src/uas/UAS.cc | 48 ++++++++++++++++++++++++++++++-------- src/ui/JoystickWidget.cc | 9 +++++++ src/ui/JoystickWidget.ui | 3 +++ 5 files changed, 80 insertions(+), 10 deletions(-) diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index b2ed15300..8dfce026d 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -72,6 +72,7 @@ void JoystickInput::loadGeneralSettings() settings.beginGroup("JOYSTICK_INPUT"); isEnabled = settings.value("ENABLED", false).toBool(); joystickName = settings.value("JOYSTICK_NAME", "").toString(); + mode = (JOYSTICK_MODE)settings.value("JOYSTICK_MODE", JOYSTICK_MODE_ATTITUDE).toInt(); settings.endGroup(); } @@ -182,6 +183,7 @@ void JoystickInput::storeGeneralSettings() const settings.beginGroup("JOYSTICK_INPUT"); settings.setValue("ENABLED", isEnabled); settings.setValue("JOYSTICK_NAME", joystickName); + settings.setValue("JOYSTICK_MODE", mode); settings.endGroup(); settings.sync(); } diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index 2d8d10fdb..9e07b9ce8 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -89,6 +89,17 @@ public: JOYSTICK_INPUT_MAPPING_THROTTLE = 4 }; + /** + * @brief The JOYSTICK_MODE enum stores the values for each item in the mode combobox. + * This should match the order of items in the mode combobox in JoystickWidget.ui. + */ + enum JOYSTICK_MODE + { + JOYSTICK_MODE_ATTITUDE = 0, + JOYSTICK_MODE_POSITION = 1, + JOYSTICK_MODE_FORCE = 2, + }; + /** * @brief Load joystick-specific settings. */ @@ -162,6 +173,11 @@ public: return numJoysticks; } + JOYSTICK_MODE getMode() const + { + return mode; + } + QString getJoystickNameById(int id) const { return QString(SDL_JoystickName(id)); @@ -203,6 +219,9 @@ protected: int joystickNumAxes; int joystickNumButtons; + // mode of joystick (attitude, position, force, ... (see JOYSTICK_MODE enum)) + JOYSTICK_MODE mode; + // Track axis/button settings based on a Joystick/AutopilotType/SystemType triplet. // This is only a double-map, because settings are stored/loaded based on joystick // name first, so only the settings for the current joystick need to be stored at any given time. @@ -341,6 +360,15 @@ public slots: * @param action The numeric ID of the action for this UAS to map to. */ void setButtonAction(int button, int action); + + /** + * @brief Specify which setpoints should be sent to the UAS when moving the joystick + * @param newMode the mode (setpoint type) see the JOYSTICK_MODE enum + */ + void setMode(int newMode) + { + mode = (JOYSTICK_MODE)newMode; + } }; #endif // _JOYSTICKINPUT_H_ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 458a8b6a3..acf18ab22 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2995,29 +2995,57 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr //const int16_t yawCommand = (int16_t)(yaw * axesScaling); //const uint16_t thrustCommand = (uint16_t)(thrust * axesScaling); - - uint8_t mode = 3; // for velocity setpoint (OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) - - // Send the MANUAL_COMMAND message mavlink_message_t message; + + //// Send the attitude setpoint external message + ////mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); + //// send an external attitude setpoint command (rate control disabled) + //float attitudeQuaternion[4]; + //mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); + //uint8_t typeMask = 0b111; // disable rate control + //mavlink_msg_attitude_setpoint_external_pack(mavlink->getSystemId(), + //mavlink->getComponentId(), + //&message, + //QGC::groundTimeUsecs(), + //this->uasId, + //0, + //typeMask, + //attitudeQuaternion, + //0, + //0, + //0, + //thrust + //); + + + // Send the the force setpoint (local pos sp external message) //mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); // send an external attitude setpoint command (rate control disabled) - float attitudeQuaternion[4]; - mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); - uint8_t typeMask = 0b111; // disable rate control - mavlink_msg_attitude_setpoint_external_pack(mavlink->getSystemId(), + float dcm[3][3]; + mavlink_euler_to_dcm(roll, pitch, yaw, dcm); + const float fx = -dcm[0][2]; + const float fy = -dcm[1][2]; + const float fz = -dcm[2][2]; + uint16_t typeMask = 0b0000001000111111; // disable rate control + mavlink_msg_local_ned_position_setpoint_external_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, QGC::groundTimeUsecs(), this->uasId, 0, + MAV_FRAME_LOCAL_NED, typeMask, - attitudeQuaternion, 0, 0, 0, - thrust + 0, + 0, + 0, + fx, + fy, + fz ); + sendMessage(message); // Emit an update in control values to other UI elements, like the HSI display diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc index 9aebe973b..106c0e641 100644 --- a/src/ui/JoystickWidget.cc +++ b/src/ui/JoystickWidget.cc @@ -45,6 +45,9 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) : styleChanged(MainWindow::instance()->getStyle()); connect(MainWindow::instance(), SIGNAL(styleChanged(MainWindow::QGC_MAINWINDOW_STYLE)), this, SLOT(styleChanged(MainWindow::QGC_MAINWINDOW_STYLE))); + // change mode when mode combobox is changed + connect(m_ui->joystickModeComboBox, SIGNAL(currentIndexChanged(int)), this->joystick, SLOT(setMode(int))); + // Display the widget above all other windows. this->raise(); this->show(); @@ -68,6 +71,12 @@ void JoystickWidget::initUI() m_ui->joystickFrame->setEnabled(true); } + // mode combo box + m_ui->joystickModeComboBox->addItem("Attitude"); + m_ui->joystickModeComboBox->addItem("Position"); + m_ui->joystickModeComboBox->addItem("Force"); + m_ui->joystickModeComboBox->setCurrentIndex(joystick->getMode()); + // Create the initial UI. createUIForJoystick(); } diff --git a/src/ui/JoystickWidget.ui b/src/ui/JoystickWidget.ui index 51b0cdf46..61634c4be 100644 --- a/src/ui/JoystickWidget.ui +++ b/src/ui/JoystickWidget.ui @@ -67,6 +67,9 @@ + + + -- 2.22.0