diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index a24e896ab435b65efb51526d8287921190eb5764..a75042bf372b60a214ac80d6f3877fbc5c781e52 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -855,10 +855,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) break; } } - unsigned char v=data[i]; - fprintf(stderr,"%02x ", v); } - fprintf(stderr,"\n"); readyBufferMutex.lock(); for (int i = 0; i < streampointer; i++) diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index b0ffb8b924b5e75f4fb6f330c59f0af2c3d2e55d..190e6ccd3940657e031dacdcdf872196af1f3991 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -382,11 +382,11 @@ static void print_message(const mavlink_message_t *msg) const mavlink_message_info_t *m = &message_info[msg->msgid]; const mavlink_field_info_t *f = m->fields; unsigned i; - qDebug("%s { ", m->name); - for (i=0; inum_fields; i++) { - print_field(msg, &f[i]); - } - qDebug("}\n"); +// qDebug("%s { ", m->name); +// for (i=0; inum_fields; i++) { +// print_field(msg, &f[i]); +// } +// qDebug("}\n"); } void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index de20f996c86c122009d7cc4a3cac8330b792202d..5c79d3a93f9620fc99cedbdc0fcaf184c1017cdb 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -28,9 +28,12 @@ JoystickInput::JoystickInput() : sdlJoystickMin(-32768.0f), sdlJoystickMax(32767.0f), + isEnabled(false), + done(false), uas(NULL), + autopilotType(0), + systemType(0), uasCanReverse(false), - done(false), rollAxis(-1), pitchAxis(-1), yawAxis(-1), @@ -39,12 +42,14 @@ JoystickInput::JoystickInput() : joystickID(-1), joystickNumButtons(0) { + for (int i = 0; i < 10; i++) { calibrationPositive[i] = sdlJoystickMax; calibrationNegative[i] = sdlJoystickMin; } - loadSettings(); + // Make sure we initialize with the correct UAS. + setActiveUAS(UASManager::instance()->getActiveUAS()); // Start this thread. This allows the Joystick Settings window to work correctly even w/o any UASes connected. start(); @@ -52,70 +57,116 @@ JoystickInput::JoystickInput() : JoystickInput::~JoystickInput() { - storeSettings(); + storeGeneralSettings(); + storeJoystickSettings(); done = true; } +void JoystickInput::loadGeneralSettings() +{ + // Load defaults from settings + QSettings settings; + settings.sync(); + + // Deal with settings specific to the JoystickInput + settings.beginGroup("JOYSTICK_INPUT"); + isEnabled = settings.value("ENABLED", false).toBool(); + QString joystickName = settings.value("JOYSTICK", "").toString(); + settings.endGroup(); +} + /** * @brief Restores settings for the current joystick from saved settings file. * Assumes that both joystickName & joystickNumAxes are correct. */ -void JoystickInput::loadSettings() +void JoystickInput::loadJoystickSettings() { // Load defaults from settings QSettings settings; settings.sync(); + + // Now for the current joystick settings.beginGroup(joystickName); rollAxis = (settings.value("ROLL_AXIS_MAPPING", -1).toInt()); pitchAxis = (settings.value("PITCH_AXIS_MAPPING", -1).toInt()); yawAxis = (settings.value("YAW_AXIS_MAPPING", -1).toInt()); throttleAxis = (settings.value("THROTTLE_AXIS_MAPPING", -1).toInt()); - // Read back the joystickAxesInverted QList one element at a time. - // Also, error check. - int axesStored = settings.beginReadArray("AXES_INVERTED"); - if (axesStored != joystickNumAxes) + // Clear out and then restore the (AUTOPILOT, SYSTEM) mapping for joystick settings + joystickSettings.clear(); + int autopilots = settings.beginReadArray("AUTOPILOTS"); + for (int i = 0; i < autopilots; i++) { - qDebug() << "Invalid number of axes for joystickAxesInverted in settings. Ignoring."; - for (int i = 0; i < joystickNumAxes; i++) - { - joystickAxesInverted.append(false); - } - } - else - { - for (int i = 0; i < joystickNumAxes; i++) + settings.setArrayIndex(i); + int autopilotType = settings.value("AUTOPILOT_TYPE", 0).toInt(); + int systems = settings.beginReadArray("SYSTEMS"); + for (int j = 0; j < systems; j++) { - settings.setArrayIndex(i); - joystickAxesInverted.append(settings.value("INVERTED", false).toBool()); - } - } - settings.endArray(); + settings.setArrayIndex(j); + int systemType = settings.value("SYSTEM_TYPE", 0).toInt(); + qDebug() << "Loading joystick settings for (" << autopilotType << "," << systemType << ")"; + + // Now that both the autopilot and system type are available, update some references. + QMap* joystickAxesInverted = &joystickSettings[autopilotType][systemType].axesInverted; + QMap* joystickAxesLimited = &joystickSettings[autopilotType][systemType].axesLimited; + QMap* joystickButtonActions = &joystickSettings[autopilotType][systemType].buttonActions; + + // Read back the joystickAxesInverted QList one element at a time. + int axesStored = settings.beginReadArray("AXES_INVERTED"); + for (int k = 0; k < axesStored; k++) + { + settings.setArrayIndex(k); + int index = settings.value("INDEX", 0).toInt(); + bool inverted = settings.value("INVERTED", false).toBool(); + joystickAxesInverted->insert(index, inverted); + qDebug() << "Loading inversion status (" << inverted << ") for axis " << index; + } + settings.endArray(); - // Read back the joystickAxesLimited QList one element at a time. - // Also, error check. - axesStored = settings.beginReadArray("AXES_LIMITED"); - if (axesStored != joystickNumAxes) - { - qDebug() << "Invalid number of axes for joystickAxesLimited in settings. Ignoring."; - for (int i = 0; i < joystickNumAxes; i++) - { - joystickAxesLimited.append(false); - } - } - else - { - for (int i = 0; i < joystickNumAxes; i++) - { - settings.setArrayIndex(i); - joystickAxesLimited.append(settings.value("LIMITED", false).toBool()); + // Read back the joystickAxesLimited QList one element at a time. + axesStored = settings.beginReadArray("AXES_LIMITED"); + for (int k = 0; k < axesStored; k++) + { + settings.setArrayIndex(k); + int index = settings.value("INDEX", 0).toInt(); + bool limited = settings.value("LIMITED", false).toBool(); + joystickAxesLimited->insert(index, limited); + qDebug() << "Loading limited status (" << limited << ") for axis " << index; + } + settings.endArray(); + + // Read back the button->action mapping. + int buttonsStored = settings.beginReadArray("BUTTONS_ACTIONS"); + for (int k = 0; k < buttonsStored; k++) + { + settings.setArrayIndex(k); + int index = settings.value("INDEX", 0).toInt(); + int action = settings.value("ACTION", 0).toInt(); + joystickButtonActions->insert(index, action); + qDebug() << "Loading action (" << action << ") for button " << index; + } + settings.endArray(); } + settings.endArray(); } settings.endArray(); + settings.endGroup(); + + emit joystickSettingsChanged(); +} + +void JoystickInput::storeGeneralSettings() const +{ + QSettings settings; + settings.beginGroup("JOYSTICK_INPUT"); + settings.setValue("ENABLED", isEnabled); + settings.setValue("JOYSTICK_NAME", joystickName); + settings.endGroup(); + settings.sync(); } -void JoystickInput::storeSettings() +void JoystickInput::storeJoystickSettings() const { // Store settings QSettings settings; @@ -124,27 +175,101 @@ void JoystickInput::storeSettings() settings.setValue("PITCH_AXIS_MAPPING", pitchAxis); settings.setValue("YAW_AXIS_MAPPING", yawAxis); settings.setValue("THROTTLE_AXIS_MAPPING", throttleAxis); - settings.beginWriteArray("AXES_INVERTED"); - for (int i = 0; i < joystickNumAxes; i++) + settings.beginWriteArray("AUTOPILOTS"); + QMapIterator > i(joystickSettings); + int autopilotIndex = 0; + while (i.hasNext()) { - settings.setArrayIndex(i); - settings.setValue("INVERTED", joystickAxesInverted.at(i)); - } - settings.endArray(); - settings.beginWriteArray("AXES_LIMITED"); - for (int i = 0; i < joystickNumAxes; i++) - { - settings.setArrayIndex(i); - settings.setValue("LIMITED", joystickAxesLimited.at(i)); + i.next(); + settings.setArrayIndex(autopilotIndex++); + + int autopilotType = i.key(); + settings.setValue("AUTOPILOT_TYPE", autopilotType); + + settings.beginWriteArray("SYSTEMS"); + QMapIterator j(i.value()); + int systemIndex = 0; + while (j.hasNext()) + { + j.next(); + settings.setArrayIndex(systemIndex++); + + int systemType = j.key(); + settings.setValue("SYSTEM_TYPE", systemType); + qDebug() << "Saving joystick settings for (" << autopilotType << "," << systemType << ")"; + + // Now that both the autopilot and system type are available, update some references. + QMapIterator joystickAxesInverted(joystickSettings[autopilotType][systemType].axesInverted); + QMapIterator joystickAxesLimited(joystickSettings[autopilotType][systemType].axesLimited); + QMapIterator joystickButtonActions(joystickSettings[autopilotType][systemType].buttonActions); + + settings.beginWriteArray("AXES_INVERTED"); + int k = 0; + while (joystickAxesInverted.hasNext()) + { + joystickAxesInverted.next(); + int inverted = joystickAxesInverted.value(); + // Only save this axes' inversion status if it's not the default + if (inverted) + { + settings.setArrayIndex(k++); + int index = joystickAxesInverted.key(); + settings.setValue("INDEX", index); + settings.setValue("INVERTED", inverted); + qDebug() << "Saving inversion status (" << inverted << ") for axis " << index; + } + } + settings.endArray(); + + settings.beginWriteArray("AXES_LIMITED"); + k = 0; + while (joystickAxesLimited.hasNext()) + { + joystickAxesLimited.next(); + int limited = joystickAxesLimited.value(); + if (limited) + { + settings.setArrayIndex(k++); + int index = joystickAxesLimited.key(); + settings.setValue("INDEX", index); + settings.setValue("LIMITED", limited); + qDebug() << "Saving limited status (" << limited << ") for axis " << index; + } + } + settings.endArray(); + + settings.beginWriteArray("BUTTONS_ACTIONS"); + k = 0; + while (joystickButtonActions.hasNext()) + { + joystickButtonActions.next(); + int action = joystickButtonActions.value(); + if (action != -1) + { + settings.setArrayIndex(k++); + int index = joystickButtonActions.key(); + settings.setValue("INDEX", index); + settings.setValue("ACTION", action); + qDebug() << "Saving action (" << action << ") for button " << index; + } + } + settings.endArray(); + } + settings.endArray(); // SYSTEMS } - settings.endArray(); + settings.endArray(); // AUTOPILOTS settings.endGroup(); settings.sync(); } - void JoystickInput::setActiveUAS(UASInterface* uas) { + // Do nothing if the UAS hasn't changed. + if (uas == this->uas) + { + return; + } + // Only connect / disconnect is the UAS is of a controllable UAS class UAS* tmp = 0; if (this->uas) @@ -153,24 +278,44 @@ void JoystickInput::setActiveUAS(UASInterface* uas) if(tmp) { disconnect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int))); - disconnect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int))); + disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int))); } uasCanReverse = false; } + // Save any settings for the last UAS + if (joystickID > -1) + { + storeJoystickSettings(); + } + this->uas = uas; - if (this->uas) + if (this->uas && (tmp = dynamic_cast(this->uas))) { - tmp = dynamic_cast(this->uas); - if(tmp) { - connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int))); - connect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int))); - uasCanReverse = tmp->systemCanReverse(); - } + connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int))); + connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int))); + uasCanReverse = tmp->systemCanReverse(); + + // Update the joystick settings for a new UAS. + autopilotType = uas->getAutopilotType(); + systemType = uas->getSystemType(); + qDebug() << "Switching to UAS with autopilot: " << autopilotType << " and system: " << systemType; + } + + // Load any joystick-specific settings now that the UAS has changed. + if (joystickID > -1) + { + loadJoystickSettings(); } } +void JoystickInput::setEnabled(bool enabled) +{ + this->isEnabled = enabled; + storeJoystickSettings(); +} + void JoystickInput::init() { // INITIALIZE SDL Joystick support @@ -207,9 +352,13 @@ void JoystickInput::init() SDL_JoystickClose(x); } - // And attach to the first joystick found to start. + // Now that we've detected a joystick, load in the joystick-agnostic settings. + loadGeneralSettings(); + + // And attach to the first joystick found to start, which also loads in joystick-specific settings. setActiveJoystick(0); + // Now make sure we know what the current UAS is and track changes to it. setActiveUAS(UASManager::instance()->getActiveUAS()); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); } @@ -247,7 +396,7 @@ void JoystickInput::run() // This is generally not used for controlling a vehicle, but a UI representation, so it being slightly off is fine. // Here we map the joystick axis value into the initial range of [0:1]. float axisValue = SDL_JoystickGetAxis(joystick, i); - if (joystickAxesInverted[i]) + if (joystickSettings[autopilotType][systemType].axesLimited[i]) { axisValue = (axisValue - calibrationNegative[i]) / (calibrationPositive[i] - calibrationNegative[i]); } @@ -264,7 +413,7 @@ void JoystickInput::run() } // Otherwise if this vehicle can only go forward, but the axis is limited to only the positive range, // scale this so the negative values are ignored for this axis and it's clamped to [0:1]. - else if (throttleAxis == i && joystickAxesLimited[i]) + else if (throttleAxis == i && joystickSettings[autopilotType][systemType].axesLimited.value(i)) { axisValue = axisValue * 2.0f - 1.0f; if (axisValue < 0.0f) @@ -311,16 +460,23 @@ void JoystickInput::run() else if (!SDL_JoystickGetButton(joystick, i) && lastButtonState) { emit buttonReleased(i); + if (isEnabled && joystickSettings[autopilotType][systemType].buttonActions.contains(i)) + { + emit actionTriggered(joystickSettings[autopilotType][systemType].buttonActions.value(i)); + } joystickButtons &= ~(1 << i); } } // Now signal an update for all UI together. - float roll = rollAxis > -1?joystickAxes[rollAxis]:NAN; - float pitch = pitchAxis > -1?joystickAxes[pitchAxis]:NAN; - float yaw = yawAxis > -1?joystickAxes[yawAxis]:NAN; - float throttle = throttleAxis > -1?joystickAxes[throttleAxis]:NAN; - emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, joystickButtons); + if (isEnabled) + { + float roll = rollAxis > -1?joystickAxes[rollAxis]:NAN; + float pitch = pitchAxis > -1?joystickAxes[pitchAxis]:NAN; + float yaw = yawAxis > -1?joystickAxes[yawAxis]:NAN; + float throttle = throttleAxis > -1?joystickAxes[throttleAxis]:NAN; + emit joystickChanged(roll, pitch, yaw, throttle, xHat, yHat, joystickButtons); + } // Sleep, update rate of joystick is approx. 50 Hz (1000 ms / 50 = 20 ms) QGC::SLEEP::msleep(20); @@ -332,7 +488,7 @@ void JoystickInput::setActiveJoystick(int id) // If we already had a joystick, close that one before opening a new one. if (joystick && SDL_JoystickOpened(joystickID)) { - storeSettings(); + storeJoystickSettings(); SDL_JoystickClose(joystick); joystick = NULL; joystickID = -1; @@ -349,19 +505,21 @@ void JoystickInput::setActiveJoystick(int id) joystickNumAxes = SDL_JoystickNumAxes(joystick); // Restore saved settings for this joystick. - loadSettings(); + loadJoystickSettings(); qDebug() << "Roll: " << rollAxis << ", Pitch: " << pitchAxis << ", Yaw: " << yawAxis << ", Throttle: " << throttleAxis; - // Update cached joystick values + // Update cached joystick axes values. + // Also emit any signals for currently-triggering events joystickAxes.clear(); - joystickAxesInverted.clear(); - joystickAxesLimited.clear(); for (int i = 0; i < joystickNumAxes; i++) { int axisValue = SDL_JoystickGetAxis(joystick, i); joystickAxes.append(axisValue); emit axisValueChanged(i, axisValue); } + + // Update cached joystick button values. + // Emit signals for any button events. joystickButtons = 0; for (int i = 0; i < joystickNumButtons; i++) { @@ -413,51 +571,72 @@ void JoystickInput::setAxisMapping(int axis, JOYSTICK_INPUT_MAPPING newMapping) if (throttleAxis == axis) { throttleAxis = -1; - joystickAxesLimited[axis] = false; + joystickSettings[autopilotType][systemType].axesLimited.remove(axis); } break; } + storeJoystickSettings(); } void JoystickInput::setAxisInversion(int axis, bool inverted) { - if (axis < joystickAxesInverted.size()) + if (axis < joystickNumAxes) { - joystickAxesInverted[axis] = inverted; + joystickSettings[autopilotType][systemType].axesInverted[axis] = inverted; + storeJoystickSettings(); } } void JoystickInput::setAxisRangeLimit(int axis, bool limitRange) { - if (axis < joystickAxesLimited.size()) + if (axis < joystickNumAxes) { - joystickAxesLimited[axis] = limitRange; + joystickSettings[autopilotType][systemType].axesLimited[axis] = limitRange; + storeJoystickSettings(); } } -float JoystickInput::getCurrentValueForAxis(int axis) +void JoystickInput::setButtonAction(int button, int action) { - if (axis < joystickAxes.size()) + if (button < joystickNumButtons) { - return joystickAxes[axis]; + joystickSettings[autopilotType][systemType].buttonActions[button] = action; + storeJoystickSettings(); + } +} + +float JoystickInput::getCurrentValueForAxis(int axis) const +{ + if (axis < joystickNumAxes) + { + return joystickAxes.at(axis); } return 0.0f; } -bool JoystickInput::getInvertedForAxis(int axis) +bool JoystickInput::getInvertedForAxis(int axis) const { - if (axis < joystickAxes.size()) + if (axis < joystickNumAxes) { - return joystickAxesInverted[axis]; + return joystickSettings[autopilotType][systemType].axesInverted.value(axis); } return false; } -bool JoystickInput::getRangeLimitForAxis(int axis) +bool JoystickInput::getRangeLimitForAxis(int axis) const { - if (axis < joystickAxes.size()) + if (axis < joystickNumAxes) { - return joystickAxesLimited[axis]; + return joystickSettings[autopilotType][systemType].axesLimited.value(axis); } return false; } + +int JoystickInput::getActionForButton(int button) const +{ + if (button < joystickNumButtons && joystickSettings[autopilotType][systemType].buttonActions.contains(button)) + { + return joystickSettings[autopilotType][systemType].buttonActions.value(button); + } + return -1; +} diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index 2216fa5a40a76c7b73b0abb6c17d82f93943af5b..2d9688c72537d81135201fc753f7c1f629df9d65 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -44,6 +44,13 @@ This file is part of the PIXHAWK project #include "UASInterface.h" +struct JoystickSettings { + QMap axesInverted; ///< Whether each axis should be used inverted from what was reported. + QMap axesLimited; ///< Whether each axis should be limited to only the positive range. Currently this only applies to the throttle axis, but is kept generic here to possibly support other axes. + QMap buttonActions; ///< The index of the action associated with every button. +}; +Q_DECLARE_METATYPE(JoystickSettings); + /** * @brief Joystick input */ @@ -53,7 +60,7 @@ class JoystickInput : public QThread public: JoystickInput(); - ~JoystickInput(); + ~JoystickInput(); void run(); void shutdown(); @@ -71,14 +78,27 @@ public: }; /** - * @brief Load joystick settings + * @brief Load joystick-specific settings. */ - void loadSettings(); + void loadJoystickSettings(); + /** + * @brief Load joystick-independent settings. + */ + void loadGeneralSettings(); /** - * @brief Store joystick settings + * @brief Store joystick-specific settings. + */ + void storeJoystickSettings() const; + /** + * @brief Store joystick-independent settings. */ - void storeSettings(); + void storeGeneralSettings() const; + + bool enabled() const + { + return isEnabled; + } int getMappingThrottleAxis() const { @@ -130,9 +150,10 @@ public: return QString(SDL_JoystickName(id)); } - float getCurrentValueForAxis(int axis); - bool getInvertedForAxis(int axis); - bool getRangeLimitForAxis(int axis); + float getCurrentValueForAxis(int axis) const; + bool getInvertedForAxis(int axis) const; + bool getRangeLimitForAxis(int axis) const; + int getActionForButton(int button) const; const double sdlJoystickMin; const double sdlJoystickMax; @@ -140,10 +161,15 @@ public: protected: double calibrationPositive[10]; double calibrationNegative[10]; + + bool isEnabled; ///< Track whether the system should emit the higher-level signals: joystickChanged & actionTriggered. + bool done; + SDL_Joystick* joystick; UASInterface* uas; ///< Track the current UAS. + int autopilotType; ///< Cache the autopilotType + int systemType; ///< Cache the systemType bool uasCanReverse; ///< Track whether the connect UAS can drive a reverse speed. - bool done; // Store the mapping between axis numbers and the roll/pitch/yaw/throttle configuration. // Value is one of JoystickAxis::JOYSTICK_INPUT_MAPPING. @@ -159,9 +185,16 @@ protected: int joystickNumAxes; int joystickNumButtons; + // 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. + // Pointers are kept to the various settings field to reduce lookup times. + // Note that the mapping (0,0) corresponds to when no UAS is connected. Since this corresponds + // to a generic vehicle type and a generic autopilot, this is a pretty safe default. + QMap > joystickSettings; + + // Track the last state of the axes, buttons, and hats for only emitting change signals. QList joystickAxes; ///< The values of every axes during the last sample. - QList joystickAxesInverted; ///< Whether each axis should be used inverted from what was reported. - QList joystickAxesLimited; ///< Whether each axis should be limited to only the positive range. quint16 joystickButtons; ///< The state of every button. Bitfield supporting 16 buttons with 1s indicating that the button is down. int xHat, yHat; ///< The horizontal/vertical hat directions. Values are -1, 0, 1, with (-1,-1) indicating bottom-left. @@ -201,6 +234,12 @@ signals: */ void buttonReleased(int key); + /** + * @brief A joystick button was pressed that had a corresponding action. + * @param action The index of the action to trigger. Dependent on UAS. + */ + void actionTriggered(int action); + /** * @brief Hat (8-way switch on the top) has changed position * @@ -218,7 +257,12 @@ signals: */ void hatDirectionChanged(int x, int y); + /** @brief Signals that new joystick-specific settings were changed. Useful for triggering updates that at dependent on the current joystick. */ + void joystickSettingsChanged(); + public slots: + /** @brief Enable or disable emitting the high-level control signals from the joystick. */ + void setEnabled(bool enable); /** @brief Specify the UAS that this input should forward joystickChanged signals and buttonPresses to. */ void setActiveUAS(UASInterface* uas); /** @brief Switch to a new joystick by ID number. Both buttons and axes are updated with the proper signals emitted. */ @@ -242,6 +286,14 @@ public slots: * @param limitRange If true only the positive half of this axis will be read. */ void setAxisRangeLimit(int axis, bool limitRange); + /** + * @brief Specify a button->action mapping for the given uas. + * This mapping is applied based on UAS autopilot type and UAS system type. + * Connects the buttonEmitted signal for the corresponding button to the corresponding action for the current UAS. + * @param button The numeric ID for the button + * @param action The numeric ID of the action for this UAS to map to. + */ + void setButtonAction(int button, int action); }; #endif // _JOYSTICKINPUT_H_ diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index f8c53de7912e1e901d0331645a86c30f02e58522..fef8bc4ebdff5fbc5cf9de63df97b3db8e568f1c 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -33,9 +33,9 @@ /** * Gets the settings from the previous UAS (name, airframe, autopilot, battery specs) -* by calling readSettings. This means the new UAS will have the same settings +* by calling readSettings. This means the new UAS will have the same settings * as the previous one created unless one calls deleteSettings in the code after -* creating the UAS. +* creating the UAS. */ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), uasId(id), @@ -159,6 +159,11 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), connect(newAction, SIGNAL(triggered()), this, SLOT(disarmSystem())); actions.append(newAction); + newAction = new QAction(tr("Toggle armed"), this); + newAction->setToolTip(tr("Toggle between armed and disarmed")); + connect(newAction, SIGNAL(triggered()), this, SLOT(toggleAutonomy())); + actions.append(newAction); + newAction = new QAction(tr("Go home"), this); newAction->setToolTip(tr("Command the UAS to return to its home position")); connect(newAction, SIGNAL(triggered()), this, SLOT(home())); @@ -204,10 +209,10 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState())); connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings())); statusTimeout->start(500); - readSettings(); + readSettings(); // Initial signals emit disarmed(); - emit armingChanged(false); + emit armingChanged(false); } /** @@ -258,7 +263,7 @@ void UAS::readSettings() /** * Deletes the settings origianally read into the UAS by readSettings. -* This is in case one does not want the old values but would rather +* This is in case one does not want the old values but would rather * start with the values assigned by the constructor. */ void UAS::deleteSettings() @@ -277,6 +282,15 @@ int UAS::getUASID() const return uasId; } +void UAS::triggerAction(int action) +{ + if (action > 0 && action < actions.size()) + { + qDebug() << "Triggering action: '" << actions[action]->text() << "'"; + actions[action]->trigger(); + } +} + /** * Update the heartbeat. */ @@ -468,15 +482,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit heartbeat(this); mavlink_heartbeat_t state; mavlink_msg_heartbeat_decode(&message, &state); - - // Send the base_mode and system_status values to the plotter. This uses the ground time - // so the Ground Time checkbox must be ticked for these values to display + + // Send the base_mode and system_status values to the plotter. This uses the ground time + // so the Ground Time checkbox must be ticked for these values to display quint64 time = getUnixTime(); - QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid); - emit valueChanged(uasId, name.arg("base_mode"), "bits", state.base_mode, time); - emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time); - emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time); - + QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid); + emit valueChanged(uasId, name.arg("base_mode"), "bits", state.base_mode, time); + emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time); + emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time); + // Set new type if it has changed if (this->type != state.type) { @@ -599,22 +613,22 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_sys_status_t state; mavlink_msg_sys_status_decode(&message, &state); - // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present. + // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present. quint64 time = getUnixTime(); - QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid); - emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time); - emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time); - emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time); - emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time); - emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time); - emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time); + QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid); + emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time); + emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time); + emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time); + emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time); + emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time); + emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time); emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time); - // Process CPU load. + // Process CPU load. emit loadChanged(this,state.load/10.0f); - emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time); + emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time); - // Battery charge/time remaining/voltage calculations + // Battery charge/time remaining/voltage calculations currentVoltage = state.voltage_battery/1000.0f; lpVoltage = filterVoltage(currentVoltage); tickLowpassVoltage = tickLowpassVoltage*0.8f + 0.2f*currentVoltage; @@ -648,16 +662,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) } emit batteryChanged(this, lpVoltage, currentCurrent, getChargeLevel(), timeRemaining); - emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time); + emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time); // emit voltageChanged(message.sysid, currentVoltage); - emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time); + emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time); - // And if the battery current draw is measured, log that also. - if (state.current_battery != -1) - { + // And if the battery current draw is measured, log that also. + if (state.current_battery != -1) + { currentCurrent = ((double)state.current_battery)/100.0f; emit valueChanged(uasId, name.arg("battery_current"), "A", currentCurrent, time); - } + } // LOW BATTERY ALARM if (lpVoltage < warnVoltage && (currentVoltage - 0.2f) < warnVoltage && (currentVoltage > 3.3)) @@ -677,17 +691,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit positionZControlEnabled(state.onboard_control_sensors_enabled & (1 << 13)); emit positionXYControlEnabled(state.onboard_control_sensors_enabled & (1 << 14)); - // Trigger drop rate updates as needed. Here we convert the incoming - // drop_rate_comm value from 1/100 of a percent in a uint16 to a true - // percentage as a float. We also cap the incoming value at 100% as defined - // by the MAVLink specifications. - if (state.drop_rate_comm > 10000) - { - state.drop_rate_comm = 10000; - } - emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f); - emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time); - } + // Trigger drop rate updates as needed. Here we convert the incoming + // drop_rate_comm value from 1/100 of a percent in a uint16 to a true + // percentage as a float. We also cap the incoming value at 100% as defined + // by the MAVLink specifications. + if (state.drop_rate_comm > 10000) + { + state.drop_rate_comm = 10000; + } + emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f); + emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time); + } break; case MAVLINK_MSG_ID_ATTITUDE: { @@ -865,13 +879,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // only accept values in a realistic range // quint64 time = getUnixTime(pos.time_usec); quint64 time = getUnixTime(pos.time_usec); - + emit gpsLocalizationChanged(this, pos.fix_type); // TODO: track localization state not only for gps but also for other loc. sources int loc_type = pos.fix_type; if (loc_type == 1) { - loc_type = 0; + loc_type = 0; } emit localizationChanged(this, loc_type); setSatelliteCount(pos.satellites_visible); @@ -1652,7 +1666,7 @@ void UAS::setLocalOriginAtCurrentGPSPosition() * @param x postion * @param y position * @param z position -*/ +*/ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) { #ifdef MAVLINK_ENABLED_PIXHAWK @@ -1672,7 +1686,7 @@ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) * @param x position * @param y position * @param z position -* @param yaw +* @param yaw */ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) { @@ -1734,8 +1748,8 @@ void UAS::startPressureCalibration() sendMessage(msg); } -/** -* Check if time is smaller than 40 years, assuming no system without Unix +/** +* Check if time is smaller than 40 years, assuming no system without Unix * timestamp runs longer than 40 years continuously without reboot. In worst case * this will add/subtract the communication delay between GCS and MAV, it will * never alter the timestamp in a safety critical way. @@ -1787,11 +1801,11 @@ quint64 UAS::getUnixReferenceTime(quint64 time) /** * @warning If attitudeStamped is enabled, this function will not actually return -* the precise time stamp of this measurement augmented to UNIX time, but will +* the precise time stamp of this measurement augmented to UNIX time, but will * MOVE the timestamp IN TIME to match the last measured attitude. There is no -* reason why one would want this, except for system setups where the onboard +* reason why one would want this, except for system setups where the onboard * clock is not present or broken and datasets should be collected that are still -* roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE +* roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE * SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL! */ quint64 UAS::getUnixTimeFromMs(quint64 time) @@ -1801,10 +1815,10 @@ quint64 UAS::getUnixTimeFromMs(quint64 time) /** * @warning If attitudeStamped is enabled, this function will not actually return -* the precise time stam of this measurement augmented to UNIX time, but will -* MOVE the timestamp IN TIME to match the last measured attitude. There is no -* reason why one would want this, except for system setups where the onboard -* clock is not present or broken and datasets should be collected that are +* the precise time stam of this measurement augmented to UNIX time, but will +* MOVE the timestamp IN TIME to match the last measured attitude. There is no +* reason why one would want this, except for system setups where the onboard +* clock is not present or broken and datasets should be collected that are * still roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED * RUINS THE SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL! */ @@ -2016,7 +2030,7 @@ QString UAS::getNavModeText(int mode) return QString("UNKNOWN"); } -/** +/** * Get the status of the code and a description of the status. * Status can be unitialized, booting up, calibrating sensors, active * standby, cirtical, emergency, shutdown or unknown. @@ -2187,7 +2201,7 @@ void UAS::readParametersFromStorage() sendMessage(msg); } -/** +/** * @param rate The update rate in Hz the message should be sent */ void UAS::enableAllDataTransmission(int rate) @@ -2215,7 +2229,7 @@ void UAS::enableAllDataTransmission(int rate) sendMessage(msg); } -/** +/** * @param rate The update rate in Hz the message should be sent */ void UAS::enableRawSensorDataTransmission(int rate) @@ -2239,7 +2253,7 @@ void UAS::enableRawSensorDataTransmission(int rate) sendMessage(msg); } -/** +/** * @param rate The update rate in Hz the message should be sent */ void UAS::enableExtendedSystemStatusTransmission(int rate) @@ -2263,7 +2277,7 @@ void UAS::enableExtendedSystemStatusTransmission(int rate) sendMessage(msg); } -/** +/** * @param rate The update rate in Hz the message should be sent */ void UAS::enableRCChannelDataTransmission(int rate) @@ -2292,7 +2306,7 @@ void UAS::enableRCChannelDataTransmission(int rate) #endif } -/** +/** * @param rate The update rate in Hz the message should be sent */ void UAS::enableRawControllerDataTransmission(int rate) @@ -2338,7 +2352,7 @@ void UAS::enableRawControllerDataTransmission(int rate) // sendMessage(msg); //} -/** +/** * @param rate The update rate in Hz the message should be sent */ void UAS::enablePositionTransmission(int rate) @@ -2362,7 +2376,7 @@ void UAS::enablePositionTransmission(int rate) sendMessage(msg); } -/** +/** * @param rate The update rate in Hz the message should be sent */ void UAS::enableExtra1Transmission(int rate) @@ -2387,7 +2401,7 @@ void UAS::enableExtra1Transmission(int rate) sendMessage(msg); } -/** +/** * @param rate The update rate in Hz the message should be sent */ void UAS::enableExtra2Transmission(int rate) @@ -2412,7 +2426,7 @@ void UAS::enableExtra2Transmission(int rate) sendMessage(msg); } -/** +/** * @param rate The update rate in Hz the message should be sent */ void UAS::enableExtra3Transmission(int rate) @@ -2441,7 +2455,7 @@ void UAS::enableExtra3Transmission(int rate) * Set a parameter value onboard * * @param component The component to set the parameter - * @param id Name of the parameter + * @param id Name of the parameter */ void UAS::setParameter(const int component, const QString& id, const QVariant& value) { @@ -2529,7 +2543,7 @@ void UAS::setParameter(const int component, const QString& id, const QVariant& v } } -/** +/** * Request parameter, use parameter name to request it. */ void UAS::requestParameter(int component, int id) @@ -2577,7 +2591,7 @@ void UAS::setSystemType(int systemType) if((systemType >= MAV_TYPE_GENERIC) && (systemType < MAV_TYPE_ENUM_END)) { type = systemType; - + // If the airframe is still generic, change it to a close default type if (airframe == 0) { @@ -2686,6 +2700,13 @@ void UAS::disarmSystem() sendMessage(msg); } +void UAS::toggleArmedState() +{ + mavlink_message_t msg; + mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode ^ MAV_MODE_FLAG_SAFETY_ARMED, navMode); + sendMessage(msg); +} + void UAS::goAutonomous() { mavlink_message_t msg; @@ -2707,8 +2728,8 @@ void UAS::toggleAutonomy() sendMessage(msg); } -/** -* Set the manual control commands. +/** +* Set the manual control commands. * This can only be done if the system has manual inputs enabled and is armed. */ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons) @@ -2809,8 +2830,8 @@ void UAS::go() sendMessage(msg); } -/** -* Order the robot to return home +/** +* Order the robot to return home */ void UAS::home() { @@ -2826,7 +2847,7 @@ void UAS::home() } /** -* Order the robot to land on the runway +* Order the robot to land on the runway */ void UAS::land() { @@ -3164,8 +3185,8 @@ const QString& UAS::getShortState() const return shortStateText; } -/** -* The mode can be autonomous, guided, manual or armed. It will also return if +/** +* The mode can be autonomous, guided, manual or armed. It will also return if * hardware in the loop is being used. * @return the audio mode text for the id given. */ @@ -3218,7 +3239,7 @@ QString UAS::getAudioModeTextFor(int id) } /** -* The mode returned can be auto, stabilized, test, manual, preflight or unknown. +* The mode returned can be auto, stabilized, test, manual, preflight or unknown. * @return the short text of the mode for the id given. */ QString UAS::getShortModeTextFor(int id) diff --git a/src/uas/UAS.h b/src/uas/UAS.h index b5dec339c73729924ba4cfc11cdf3448a3d05475..85b8b250dfdf747b30c9147e6f2a4edea1169a2d 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -699,7 +699,7 @@ public slots: this->airframe = airframe; emit systemSpecsChanged(uasId); } - + } /** @brief Set a new name **/ void setUASName(const QString& name); @@ -782,6 +782,8 @@ public slots: void armSystem(); /** @brief Disable the motors */ void disarmSystem(); + /** @brief Toggle the armed state of the system. */ + void toggleArmedState(); /** * @brief Tell the UAS to switch into a completely-autonomous mode, so disable manual input. */ @@ -883,6 +885,9 @@ public slots: void startDataRecording(); void stopDataRecording(); void deleteSettings(); + + /** @brief Triggers the action associated with the given ID. */ + void triggerAction(int action); signals: /** @brief The main/battery voltage has changed/was updated */ //void voltageChanged(int uasId, double voltage); // Defined in UASInterface already diff --git a/src/ui/JoystickButton.cc b/src/ui/JoystickButton.cc index a3df7a75969e051e5ced9e757d37969f826ecd32..053d11b10e2b731da3c0e392a33163b53cb836e4 100644 --- a/src/ui/JoystickButton.cc +++ b/src/ui/JoystickButton.cc @@ -1,5 +1,6 @@ #include "JoystickButton.h" #include "ui_JoystickButton.h" +#include "UASManager.h" JoystickButton::JoystickButton(int id, QWidget *parent) : QWidget(parent), @@ -8,6 +9,7 @@ JoystickButton::JoystickButton(int id, QWidget *parent) : { m_ui->setupUi(this); m_ui->joystickButtonLabel->setText(QString::number(id)); + this->setActiveUAS(UASManager::instance()->getActiveUAS()); connect(m_ui->joystickAction, SIGNAL(currentIndexChanged(int)), this, SLOT(actionComboBoxChanged(int))); } @@ -18,6 +20,8 @@ JoystickButton::~JoystickButton() void JoystickButton::setActiveUAS(UASInterface* uas) { + // Disable signals so that changes to joystickAction don't trigger JoystickInput updates. + blockSignals(true); if (uas) { m_ui->joystickAction->setEnabled(true); @@ -33,9 +37,20 @@ void JoystickButton::setActiveUAS(UASInterface* uas) m_ui->joystickAction->clear(); m_ui->joystickAction->addItem("--"); } + blockSignals(false); +} + +void JoystickButton::setAction(int index) +{ + // Disable signals so that changes to joystickAction don't trigger JoystickInput updates. + //blockSignals(true); + // Add one because the default no-action takes the 0-spot. + m_ui->joystickAction->setCurrentIndex(index + 1); + //blockSignals(false); } void JoystickButton::actionComboBoxChanged(int index) { - emit actionChanged(id, index); + // Subtract one because the default no-action takes the 0-spot. + emit actionChanged(id, index - 1); } diff --git a/src/ui/JoystickButton.h b/src/ui/JoystickButton.h index cf49e70ea76ee42dcd41c73808aff3423237971b..92fed4e285c1c107de8ebc38f69c467a875d9e49 100644 --- a/src/ui/JoystickButton.h +++ b/src/ui/JoystickButton.h @@ -21,6 +21,8 @@ public: public slots: /** @brief Specify the UAS that this axis should track for displaying throttle properly. */ void setActiveUAS(UASInterface* uas); + /** @brieft Specify which action this button should correspond to. */ + void setAction(int index); signals: /** @brief Signal a change in this buttons' action mapping */ diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc index 3dae94b5f183ec0ecd885a5ca5821d4b6fdc926a..2ae22c08c56d575d057b7175bb2b91eb313c0194 100644 --- a/src/ui/JoystickWidget.cc +++ b/src/ui/JoystickWidget.cc @@ -27,11 +27,18 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) : connect(this->joystick, SIGNAL(axisValueChanged(int,float)), this, SLOT(updateAxisValue(int,float))); connect(this->joystick, SIGNAL(hatDirectionChanged(int,int)), this, SLOT(setHat(int,int))); - // Update the UI if the joystick changes. - connect(m_ui->joystickNameComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(updateUIForJoystick(int))); + // Also watch for when new settings were loaded for the current joystick to do a mass UI refresh. + connect(this->joystick, SIGNAL(joystickSettingsChanged()), this, SLOT(updateUI())); - // Enable/disable the UI based on the enable checkbox + // If the selected joystick is changed, update the joystick and the UI. + connect(m_ui->joystickNameComboBox, SIGNAL(currentIndexChanged(int)), this->joystick, SLOT(setActiveJoystick(int))); + connect(m_ui->joystickNameComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(createUIForJoystick())); + + // Initialize the UI to the current JoystickInput state. Also make sure to listen for future changes + // so that the UI can be updated. connect(m_ui->enableCheckBox, SIGNAL(toggled(bool)), m_ui->joystickFrame, SLOT(setEnabled(bool))); + m_ui->enableCheckBox->setChecked(this->joystick->enabled()); // Needs to be after connecting to the joystick frame and before watching for enabled events from JoystickInput. + connect(m_ui->enableCheckBox, SIGNAL(toggled(bool)), this->joystick, SLOT(setEnabled(bool))); // Update the button label colors based on the current theme and watch for future theme changes. styleChanged(MainWindow::instance()->getStyle()); @@ -69,7 +76,7 @@ void JoystickWidget::initUI() } // Add any missing buttons - updateUIForJoystick(joystick->getJoystickID()); + createUIForJoystick(); } void JoystickWidget::styleChanged(MainWindow::QGC_MAINWINDOW_STYLE newStyle) @@ -100,7 +107,51 @@ void JoystickWidget::changeEvent(QEvent *e) } } -void JoystickWidget::updateUIForJoystick(int id) +void JoystickWidget::updateUI() +{ + for (int i = 0; i < buttons.size(); i++) + { + JoystickButton* button = buttons[i]; + int action = joystick->getActionForButton(i); + button->setAction(action + 1); + qDebug() << "JoystickWidget: Updating button " << i << " to action " << action + 1; + } + int rollAxis = joystick->getMappingRollAxis(); + int pitchAxis = joystick->getMappingPitchAxis(); + int yawAxis = joystick->getMappingYawAxis(); + int throttleAxis = joystick->getMappingThrottleAxis(); + for (int i = 0; i < axes.size(); i++) + { + JoystickAxis* axis = axes[i]; + float value = joystick->getCurrentValueForAxis(i); + axis->setValue(value); + JoystickInput::JOYSTICK_INPUT_MAPPING mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_NONE; + if (i == rollAxis) + { + mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_ROLL; + } + else if (i == pitchAxis) + { + mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_PITCH; + } + else if (i == yawAxis) + { + mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_YAW; + } + else if (i == throttleAxis) + { + mapping = JoystickInput::JOYSTICK_INPUT_MAPPING_THROTTLE; + } + axis->setMapping(mapping); + bool inverted = joystick->getInvertedForAxis(i); + axis->setInverted(inverted); + bool limited = joystick->getRangeLimitForAxis(i); + axis->setRangeLimit(limited); + qDebug() << "JoystickWidget: Updating axis " << i << " to value:" << value << ", mapping:" << mapping << ", inverted:" << inverted << ", limited:" << limited; + } +} + +void JoystickWidget::createUIForJoystick() { // Delete all the old UI elements foreach (JoystickButton* b, buttons) @@ -114,9 +165,6 @@ void JoystickWidget::updateUIForJoystick(int id) } axes.clear(); - // Set the JoystickInput to listen to the new joystick instead. - joystick->setActiveJoystick(id); - // And add the necessary button displays for this joystick. int newButtons = joystick->getJoystickNumButtons(); if (newButtons) @@ -125,6 +173,8 @@ void JoystickWidget::updateUIForJoystick(int id) for (int i = 0; i < newButtons; i++) { JoystickButton* button = new JoystickButton(i, m_ui->buttonBox); + button->setAction(joystick->getActionForButton(i)); + connect(button, SIGNAL(actionChanged(int,int)), this->joystick, SLOT(setButtonAction(int,int))); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), button, SLOT(setActiveUAS(UASInterface*))); m_ui->buttonLayout->addWidget(button); buttons.append(button); diff --git a/src/ui/JoystickWidget.h b/src/ui/JoystickWidget.h index 624ec87f2434fc9218ffbe2e1e30c45c9d700cd2..578701fc917eeda10ad33fc926dfecfe4b2c8917 100644 --- a/src/ui/JoystickWidget.h +++ b/src/ui/JoystickWidget.h @@ -54,7 +54,7 @@ public: public slots: /** @brief Update the UI for a new joystick based on SDL ID. */ - void updateUIForJoystick(int id); + void createUIForJoystick(); /** * @brief Update a given axis with a new value * @param axis The index of the axis to update. @@ -72,6 +72,8 @@ public slots: void joystickButtonReleased(int key); /** @brief Update the UI color scheme when the MainWindow theme changes. */ void styleChanged(MainWindow::QGC_MAINWINDOW_STYLE); + /** Update the UI assuming the joystick has stayed the same. */ + void updateUI(); protected: /** @brief Update the proper number of buttons for the current joystick. */ diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc index f0ce7951216cfdac89b924ed655711f64c85183f..d109847dc4140641f1d36b3e39e16d0f67dc4cd7 100644 --- a/src/ui/uas/UASListWidget.cc +++ b/src/ui/uas/UASListWidget.cc @@ -123,7 +123,7 @@ void UASListWidget::updateStatus() { displayString = QString("%1").arg(i.key()->getName()) + displayString; } - qDebug() << p << ": " + displayString; +// qDebug() << p << ": " + displayString; i.value()->setToolTip(displayString); } }