UAS: send swarm messages instead of manual control

src/uas/UAS.cc

@@ -2930,17 +2930,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