Have settings loaded/stored correctly for all joysticks based on their UAS...

Have settings loaded/stored correctly for all joysticks based on their UAS type and autopilot type. Switching between UASes doesn't work right yet, however.

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++)

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; i<m->num_fields; i++) {
-        print_field(msg, &f[i]);
-    }
-    qDebug("}\n");
+//    qDebug("%s { ", m->name);
+//    for (i=0; i<m->num_fields; i++) {
+//        print_field(msg, &f[i]);
+//    }
+//    qDebug("}\n");
 }
 
 void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)

src/input/JoystickInput.h

@@ -44,6 +44,13 @@ This file is part of the PIXHAWK project
 
 #include "UASInterface.h"
 
+struct JoystickSettings {
+    QMap<int, bool> axesInverted; ///< Whether each axis should be used inverted from what was reported.
+    QMap<int, bool> 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<int, int> 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<int, QMap<int, JoystickSettings> > joystickSettings;
+
+    // Track the last state of the axes, buttons, and hats for only emitting change signals.
     QList<float> joystickAxes; ///< The values of every axes during the last sample.
-    QList<bool> joystickAxesInverted; ///< Whether each axis should be used inverted from what was reported.
-    QList<bool> 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_

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

src/ui/JoystickButton.cc

 #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);
 }

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 */

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);

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. */

src/ui/uas/UASListWidget.cc

@@ -123,7 +123,7 @@ void UASListWidget::updateStatus()
         {
             displayString = QString("<b>%1</b>").arg(i.key()->getName()) + displayString;
         }
-        qDebug() << p << ": " + displayString;
+//        qDebug() << p << ": " + displayString;
         i.value()->setToolTip(displayString);
     }
 }