Merge remote-tracking branch 'julian/joystickinput' into mavlinksubmodule_joystickinput

Conflicts:
	libs/mavlink/include/mavlink/v1.0

src/input/JoystickInput.h

@@ -37,6 +37,7 @@ This file is part of the PIXHAWK project
  *
  * @author Lorenz Meier <mavteam@student.ethz.ch>
  * @author Andreas Romer <mavteam@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef _JOYSTICKINPUT_H_
@@ -56,6 +57,8 @@ This file is part of the PIXHAWK project
 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, float> axesMaxRange; ///< The maximum values per axis
+    QMap<int, float> axesMinRange; ///< The minimum values per axis
     QMap<int, int> buttonActions; ///< The index of the action associated with every button.
 };
 Q_DECLARE_METATYPE(JoystickSettings)
@@ -109,6 +112,11 @@ public:
         return isEnabled;
     }
 
+    bool calibrating() const
+    {
+        return isCalibrating;
+    }
+
     int getMappingThrottleAxis() const
     {
         return throttleAxis;
@@ -162,16 +170,17 @@ public:
     float getCurrentValueForAxis(int axis) const;
     bool getInvertedForAxis(int axis) const;
     bool getRangeLimitForAxis(int axis) const;
+    float getAxisRangeLimitMinForAxis(int axis) const;
+    float getAxisRangeLimitMaxForAxis(int axis) const;
     int getActionForButton(int button) const;
 
     const double sdlJoystickMin;
     const double sdlJoystickMax;
 
 protected:
-    double calibrationPositive[10];
-    double calibrationNegative[10];
 
     bool isEnabled; ///< Track whether the system should emit the higher-level signals: joystickChanged & actionTriggered.
+    bool isCalibrating; ///< Track if calibration in progress
     bool done;
 
     SDL_Joystick* joystick;
@@ -287,6 +296,8 @@ public slots:
     void setActiveUAS(UASInterface* uas);
     /** @brief Switch to a new joystick by ID number. Both buttons and axes are updated with the proper signals emitted. */
     void setActiveJoystick(int id);
+    /** @brief Switch calibration mode active */
+    void setCalibrating(bool active);
     /**
      * @brief Change the control mapping for a given joystick axis.
      * @param axisID The axis to modify (0-indexed)
@@ -300,12 +311,28 @@ public slots:
      * @param inverted True indicates inverted from normal. Varies by controller.
      */
     void setAxisInversion(int axis, bool inverted);
+
     /**
      * @brief Specify that an axis should only transmit the positive values. Useful for controlling throttle from auto-centering axes.
      * @param axis Which axis has its range limited.
      * @param limitRange If true only the positive half of this axis will be read.
      */
     void setAxisRangeLimit(int axis, bool limitRange);
+
+    /**
+     * @brief Specify minimum value for axis.
+     * @param axis Which axis should be set.
+     * @param min Value to be set.
+     */
+    void setAxisRangeLimitMin(int axis, float min);
+
+    /**
+     * @brief Specify maximum value for axis.
+     * @param axis Which axis should be set.
+     * @param max Value to be set.
+     */
+    void setAxisRangeLimitMax(int axis, float max);
+
     /**
      * @brief Specify a button->action mapping for the given uas.
      * This mapping is applied based on UAS autopilot type and UAS system type.

src/uas/UAS.cc

@@ -2947,9 +2947,11 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr
     static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission
 
     // We only transmit manual command messages if the system has manual inputs enabled and is armed
-    if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
-    {
+    //if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
 
+    // Lets always send velocity commands for now
+    if (true)
+    {
         // Transmit the manual commands only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with
         // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate
         // if no command inputs have changed.
@@ -2981,17 +2983,27 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr
             manualButtons = buttons;
 
             // Store scaling values for all 3 axes
-            const float axesScaling = 1.0 * 1000.0;
+            const float axesScaling = 1000.0f;
 
             // Calculate the new commands for roll, pitch, yaw, and thrust
-            const float newRollCommand = roll * axesScaling;
-            const float newPitchCommand = pitch * axesScaling;
-            const float newYawCommand = yaw * axesScaling;
-            const float newThrustCommand = thrust * axesScaling;
+//            const float newRollCommand = roll * axesScaling;
+//            const float newPitchCommand = pitch * axesScaling;
+//            const float newYawCommand = yaw * axesScaling;
+//            const float newThrustCommand = thrust * axesScaling;
+            const int16_t rollCommand = (int16_t)(roll * axesScaling);
+            const int16_t pitchCommand = (int16_t)(pitch * axesScaling);
+            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;
-            mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
+            //mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons);
+            // hack to send swarm command, TODO: replace with proper message
+            mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, mode,
+                                                                  &rollCommand, &pitchCommand, &yawCommand, &thrustCommand);
             sendMessage(message);
 
             // Emit an update in control values to other UI elements, like the HSI display

src/ui/JoystickAxis.h

@@ -45,6 +45,8 @@ public:
     void setMapping(JoystickInput::JOYSTICK_INPUT_MAPPING newMapping);
     void setInverted(bool newValue);
     void setRangeLimit(bool newValue);
+    void setAxisRangeMin(float min);
+    void setAxisRangeMax(float max);
 
 signals:
     /** @brief Signal a change in this axis' yaw/pitch/roll mapping */

src/ui/JoystickWidget.cc

@@ -228,6 +228,8 @@ void JoystickWidget::createUIForJoystick()
     {
         m_ui->axesBox->hide();
     }
+
+    connect(m_ui->calibrationButton, SIGNAL(clicked()), this, SLOT(cycleCalibrationButton()));
 }
 
 void JoystickWidget::updateAxisValue(int axis, float value)
@@ -253,3 +255,14 @@ void JoystickWidget::joystickButtonReleased(int key)
 {
     buttons.at(key)->setStyleSheet("");
 }
+
+void JoystickWidget::cycleCalibrationButton()
+{
+    if (this->joystick->calibrating()) {
+        this->joystick->setCalibrating(false);
+        m_ui->calibrationButton->setText("Calibrate range");
+    } else {
+        this->joystick->setCalibrating(true);
+        m_ui->calibrationButton->setText("End calibration");
+    }
+}

src/ui/JoystickWidget.h

@@ -70,6 +70,8 @@ public slots:
     void joystickButtonPressed(int key);
     /** @brief Trigger a UI change based on a button being released */
     void joystickButtonReleased(int key);
+    /** @brief Toggle the calibration button */
+    void cycleCalibrationButton();
     /** @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. */

src/ui/JoystickWidget.ui

@@ -78,7 +78,7 @@
      <property name="frameShadow">
       <enum>QFrame::Sunken</enum>
      </property>
-     <layout class="QVBoxLayout" name="verticalLayout_2" stretch="0,0">
+     <layout class="QVBoxLayout" name="verticalLayout_2" stretch="0,0,0,0">
       <property name="sizeConstraint">
        <enum>QLayout::SetMinimumSize</enum>
       </property>
@@ -171,6 +171,20 @@
         </property>
        </widget>
       </item>
+      <item>
+       <widget class="QPushButton" name="calibrationButton">
+        <property name="text">
+         <string>Calibrate range</string>
+        </property>
+       </widget>
+      </item>
+      <item>
+       <widget class="QLabel" name="calibrationStatusLabel">
+        <property name="text">
+         <string>Range status</string>
+        </property>
+       </widget>
+      </item>
      </layout>
     </widget>
    </item>