UAS: some cleanup for setManualControlCommands and set idle rate to 5Hz instead of 2.5Hz

src/uas/UAS.cc

@@ -2946,18 +2946,13 @@ 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
+    // 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.
-        // The default transmission rate is 50Hz, but when no inputs have changed it drops down to 5Hz.
+
+    // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz.
     bool sendCommand = false;
-        if (countSinceLastTransmission++ >= 10)
+    if (countSinceLastTransmission++ >= 5)
     {
         sendCommand = true;
         countSinceLastTransmission = 0;
@@ -2982,25 +2977,8 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr
         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);
@@ -3023,7 +3001,6 @@ void UAS::setManualControlCommands(float roll, float pitch, float yaw, float thr
         // Emit an update in control values to other UI elements, like the HSI display
         emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
     }
-    }
 }
 
 void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)