Merge pull request #173 from squilter/add_gimbal_support

Add gimbal support

Author: m4gr3d

ClientLib/src/main/java/com/o3dr/android/client/apis/drone/camera/GimbalApi.java

@@ -0,0 +1,17 @@
+package com.o3dr.android.client.apis.drone.camera;
+
+import android.os.Bundle;
+
+import com.o3dr.android.client.Drone;
+import com.o3dr.services.android.lib.model.action.Action;
+import static com.o3dr.services.android.lib.drone.action.GimbalActions.*;
+public class GimbalApi {
+
+    public static boolean setGimbalOrientation(Drone drone, double pitch, double roll, double yaw){
+        Bundle params = new Bundle();
+        params.putDouble(GIMBAL_PITCH, pitch);
+        params.putDouble(GIMBAL_ROLL, roll);
+        params.putDouble(GIMBAL_YAW, yaw);
+        return drone != null && drone.performAsyncAction(new Action(ACTION_SET_GIMBAL_ORIENTATION, params));
+    }
+}

ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/GimbalActions.java

@@ -0,0 +1,13 @@
+package com.o3dr.services.android.lib.drone.action;
+
+import com.o3dr.services.android.lib.util.Utils;
+
+public class GimbalActions {
+    public static final String GIMBAL_PITCH = "gimbal_pitch";
+    public static final String GIMBAL_YAW = "gimbal_yaw";
+    public static final String GIMBAL_ROLL = "gimbal_roll";
+
+    public static final String ACTION_SET_GIMBAL_ORIENTATION = Utils.PACKAGE_NAME + ".action.gimbal" +
+            ".SET_GIMBAL_ORIENTATION";
+
+}

ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java

@@ -14,10 +14,12 @@
 import com.MAVLink.ardupilotmega.msg_mag_cal_progress;
 import com.MAVLink.ardupilotmega.msg_mag_cal_report;
 import com.MAVLink.enums.MAG_CAL_STATUS;
+import com.o3dr.android.client.apis.drone.camera.GimbalApi;
 import com.o3dr.services.android.lib.coordinate.LatLong;
 import com.o3dr.services.android.lib.coordinate.LatLongAlt;
 import com.o3dr.services.android.lib.drone.action.ConnectionActions;
 import com.o3dr.services.android.lib.drone.action.ExperimentalActions;
+import com.o3dr.services.android.lib.drone.action.GimbalActions;
 import com.o3dr.services.android.lib.drone.action.GuidedActions;
 import com.o3dr.services.android.lib.drone.action.ParameterActions;
 import com.o3dr.services.android.lib.drone.action.StateActions;
@@ -490,6 +492,13 @@ public void performAction(Action action) throws RemoteException {
             case CameraActions.ACTION_STOP_VIDEO_RECORDING:
                 DroneApiUtils.stopVideoRecording(getDrone());
                 break;
+
+            case GimbalActions.ACTION_SET_GIMBAL_ORIENTATION:
+                double pitch = data.getDouble(GimbalActions.GIMBAL_PITCH);
+                double roll = data.getDouble(GimbalActions.GIMBAL_ROLL);
+                double yaw = data.getDouble(GimbalActions.GIMBAL_YAW);
+                MavLinkDoCmds.setGimbalOrientation(getDrone(), pitch, roll, yaw);
+                break;
         }
     }
 

dependencyLibs/Core/src/org/droidplanner/core/MAVLink/command/doCmd/MavLinkDoCmds.java

@@ -98,4 +98,27 @@ public static void setServo(Drone drone, int channel, int pwm) {
         drone.getMavClient().sendMavPacket(msg.pack());
     }
 
+    /**
+     * Set the orientation of a gimbal
+     *
+     * @param drone       target vehicle
+     * @param pitch       the desired gimbal pitch in degrees
+     * @param roll       the desired gimbal roll in degrees
+     * @param yaw       the desired gimbal yaw in degrees
+     */
+    public static void setGimbalOrientation(Drone drone, double pitch, double roll, double yaw) {
+        if (drone == null)
+            return;
+
+        msg_command_long msg = new msg_command_long();
+        msg.target_system = drone.getSysid();
+        msg.target_component = drone.getCompid();
+        msg.command = MAV_CMD.MAV_CMD_DO_MOUNT_CONTROL;
+        msg.param1 = (int) (pitch * 100);
+        msg.param2 = (int) (roll * 100);
+        msg.param3 = (int) (yaw * 100);
+
+        drone.getMavClient().sendMavPacket(msg.pack());
+    }
+
 }