Merge pull request #714 from Susurrus/joystick

Reduce the transmission rate of MANUAL_CONTROL messages when command inputs have not changed

src/input/JoystickInput.cc

@@ -272,7 +272,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas)
         tmp = dynamic_cast<UAS*>(this->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(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16)));
             disconnect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
         }
         uasCanReverse = false;
@@ -288,7 +288,7 @@ void JoystickInput::setActiveUAS(UASInterface* uas)
 
     if (this->uas && (tmp = dynamic_cast<UAS*>(this->uas)))
     {
-        connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double,int,int,int)));
+        connect(this, SIGNAL(joystickChanged(float,float,float,float,qint8,qint8,quint16)), tmp, SLOT(setManualControlCommands(float,float,float,float,qint8,qint8,quint16)));
         connect(this, SIGNAL(actionTriggered(int)), tmp, SLOT(triggerAction(int)));
         uasCanReverse = tmp->systemCanReverse();
 
@@ -439,10 +439,10 @@ void JoystickInput::run()
 
         // Build up vectors describing the hat position
         int hatPosition = SDL_JoystickGetHat(joystick, 0);
-        int newYHat = 0;
+        qint8 newYHat = 0;
         if ((SDL_HAT_UP & hatPosition) > 0) newYHat = 1;
         if ((SDL_HAT_DOWN & hatPosition) > 0) newYHat = -1;
-        int newXHat = 0;
+        qint8 newXHat = 0;
         if ((SDL_HAT_LEFT & hatPosition) > 0) newXHat = -1;
         if ((SDL_HAT_RIGHT & hatPosition) > 0) newXHat = 1;
         if (newYHat != yHat || newXHat != xHat)

src/input/JoystickInput.h

@@ -205,7 +205,7 @@ protected:
     // 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.
     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.
+    qint8 xHat, yHat;            ///< The horizontal/vertical hat directions. Values are -1, 0, 1, with (-1,-1) indicating bottom-left.
 
     /**
      * @brief Called before main run() event loop starts. Waits for joysticks to be connected.
@@ -224,7 +224,7 @@ signals:
      * @param xHat hat vector in forward-backward direction, +1 forward, 0 center, -1 backward
      * @param yHat hat vector in left-right direction, -1 left, 0 center, +1 right
      */
-    void joystickChanged(double roll, double pitch, double yaw, double throttle, int xHat, int yHat, int buttons);
+    void joystickChanged(float roll, float pitch, float yaw, float throttle, qint8 xHat, qint8 yHat, quint16 buttons);
 
     /**
       * @brief Emit a new value for an axis
@@ -267,7 +267,7 @@ signals:
       * @param x vector in left-right direction
       * @param y vector in forward-backward direction
       */
-    void hatDirectionChanged(int x, int y);
+    void hatDirectionChanged(qint8 x, qint8 y);
 
     /** @brief Signal that the UAS has been updated for this JoystickInput
      * Note that any UI updates should NOT query this object for joystick details. That should be done in response to the joystickSettingsChanged signal.

src/uas/UAS.cc

@@ -2880,34 +2880,70 @@ void UAS::toggleAutonomy()
 * 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)
+void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons)
 {
     Q_UNUSED(xHat);
     Q_UNUSED(yHat);
 
-    // Scale values
-    double rollPitchScaling = 1.0f * 1000.0f;
-    double yawScaling = 1.0f * 1000.0f;
-    double thrustScaling = 1.0f * 1000.0f;
+    // Store the previous manual commands
+    static float manualRollAngle = 0.0;
+    static float manualPitchAngle = 0.0;
+    static float manualYawAngle = 0.0;
+    static float manualThrust = 0.0;
+    static quint16 manualButtons = 0;
+    static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission
 
-    manualRollAngle = roll * rollPitchScaling;
-    manualPitchAngle = pitch * rollPitchScaling;
-    manualYawAngle = yaw * yawScaling;
-    manualThrust = thrust * thrustScaling;
-
-    // If system has manual inputs enabled and is armed
+    // 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))
     {
-        mavlink_message_t message;
-        mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualPitchAngle, (float)manualRollAngle, (float)manualThrust, (float)manualYawAngle, buttons);
-        sendMessage(message);
-        //qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
 
-        emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
-    }
-    else
-    {
-        //qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first";
+        // 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.
+        // The default transmission rate is 50Hz, but when no inputs have changed it drops down to 5Hz.
+        bool sendCommand = false;
+        if (countSinceLastTransmission++ >= 10)
+        {
+            sendCommand = true;
+            countSinceLastTransmission = 0;
+        }
+        else if ((!isnan(roll) && roll != manualRollAngle) || (!isnan(pitch) && pitch != manualPitchAngle) ||
+                   (!isnan(yaw) && yaw != manualYawAngle) || (!isnan(thrust) && thrust != manualThrust) ||
+                   buttons != manualButtons)
+        {
+            sendCommand = true;
+
+            // Ensure that another message will be sent the next time this function is called
+            countSinceLastTransmission = 10;
+        }
+
+        // Now if we should trigger an update, let's do that
+        if (sendCommand)
+        {
+            // Save the new manual control inputs
+            manualRollAngle = roll;
+            manualPitchAngle = pitch;
+            manualYawAngle = yaw;
+            manualThrust = thrust;
+            manualButtons = buttons;
+
+            // Store scaling values for all 3 axes
+            const float axesScaling = 1.0 * 1000.0;
+
+            // 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;
+
+            // 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);
+            sendMessage(message);
+
+            // Emit an update in control values to other UI elements, like the HSI display
+            emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
+        }
     }
 }
 

src/uas/UAS.h

@@ -838,7 +838,7 @@ public slots:
     void toggleAutonomy();
 
     /** @brief Set the values for the manual control of the vehicle */
-    void setManualControlCommands(double roll, double pitch, double yaw, double thrust, int xHat, int yHat, int buttons);
+    void setManualControlCommands(float roll, float pitch, float yaw, float thrust, qint8 xHat, qint8 yHat, quint16 buttons);
 
     /** @brief Set the values for the 6dof manual control of the vehicle */
     void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw);

src/uas/UASInterface.h

@@ -524,7 +524,7 @@ signals:
     void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
     void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec);
     void attitudeRotationRatesChanged(int uas, double rollrate, double pitchrate, double yawrate, quint64 usec);
-    void attitudeThrustSetPointChanged(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
+    void attitudeThrustSetPointChanged(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec);
     /** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */
     void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
     /** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */

src/ui/HSIDisplay.cc

@@ -921,7 +921,7 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
         disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-        disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
+        disconnect(this->uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64)));
         disconnect(this->uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
         disconnect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
         disconnect(this->uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
@@ -957,8 +957,8 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
                 this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)),
                 this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-        connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)),
-                this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
+        connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,float,float,float,float,quint64)),
+                this, SLOT(updateAttitudeSetpoints(UASInterface*,float,float,float,float,quint64)));
         connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)),
                 this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
         connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)),
@@ -1100,7 +1100,7 @@ void HSIDisplay::sendBodySetPointCoordinates()
     }
 }
 
-void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec)
+void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec)
 {
     Q_UNUSED(uas);
     Q_UNUSED(usec);

src/ui/HSIDisplay.h

@@ -55,7 +55,7 @@ public slots:
     /** @brief Set the width in meters this widget shows from top */
     void setMetricWidth(double width);
     void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
-    void updateAttitudeSetpoints(UASInterface*, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec);
+    void updateAttitudeSetpoints(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec);
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 time);
     void updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
     void updatePositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);

src/ui/JoystickWidget.cc

@@ -25,7 +25,7 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) :
     connect(this->joystick, SIGNAL(buttonPressed(int)), this, SLOT(joystickButtonPressed(int)));
     connect(this->joystick, SIGNAL(buttonReleased(int)), this, SLOT(joystickButtonReleased(int)));
     connect(this->joystick, SIGNAL(axisValueChanged(int,float)), this, SLOT(updateAxisValue(int,float)));
-    connect(this->joystick, SIGNAL(hatDirectionChanged(int,int)), this, SLOT(setHat(int,int)));
+    connect(this->joystick, SIGNAL(hatDirectionChanged(qint8,qint8)), this, SLOT(setHat(qint8,qint8)));
 
     // 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()));
@@ -238,7 +238,7 @@ void JoystickWidget::updateAxisValue(int axis, float value)
     }
 }
 
-void JoystickWidget::setHat(int x, int y)
+void JoystickWidget::setHat(qint8 x, qint8 y)
 {
     m_ui->statusLabel->setText(tr("Hat position: x: %1, y: %2").arg(x).arg(y));
 }

src/ui/JoystickWidget.h

@@ -65,7 +65,7 @@ public slots:
     /** @brief Update the UI with new values for the hat.
      *  @see JoystickInput::hatDirectionChanged
      */
-    void setHat(int x, int y);
+    void setHat(qint8 x, qint8 y);
     /** @brief Trigger a UI change based on a button being pressed */
     void joystickButtonPressed(int key);
     /** @brief Trigger a UI change based on a button being released */