diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 458a8b6a39bdadffb6e573f68db7e03197847218..3edd4d087d9ddf842aa419bca817292cd4cce261 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -2946,83 +2946,60 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr
     static quint16 manualButtons = 0;
     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))
-
-    // 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.
-        // 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;
+    // Transmit the external setpoints 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.
 
-            // Ensure that another message will be sent the next time this function is called
-            countSinceLastTransmission = 10;
-        }
+    // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz.
+    bool sendCommand = false;
+    if (countSinceLastTransmission++ >= 5)
+    {
+        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;
 
-        // 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 = 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 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);
-            // 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
-                    );
-            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());
-        }
+        // 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;
+
+        mavlink_message_t message;
+
+        // 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
+            );
+        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());
     }
 }