Merge pull request #317 from dronekit/improve_control_api

Control Api updates.

ClientLib/build.gradle

@@ -5,7 +5,7 @@ ext {
     VERSION_MINOR = 6
     VERSION_PATCH = 8
     VERSION_BUILD = 0
-    VERSION_SUFFIX = "beta2"
+    VERSION_SUFFIX = "release"
 
     PUBLISH_ARTIFACT_ID = 'dronekit-android'
     PUBLISH_VERSION = generateVersionName("", VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH, VERSION_SUFFIX)

ClientLib/src/main/java/com/o3dr/android/client/Drone.java

@@ -233,17 +233,23 @@ public void post(Runnable action) {
         handler.post(action);
     }
 
+    /**
+     * Reset the vehicle flight timer.
+     */
     public void resetFlightTimer() {
         elapsedFlightTime = 0;
         startTime = SystemClock.elapsedRealtime();
     }
 
-    public void stopTimer() {
+    private void stopTimer() {
         // lets calc the final elapsed timer
         elapsedFlightTime += SystemClock.elapsedRealtime() - startTime;
         startTime = SystemClock.elapsedRealtime();
     }
 
+    /**
+     * @return Vehicle flight time in seconds.
+     */
     public long getFlightTime() {
         State droneState = getAttribute(AttributeType.STATE);
         if (droneState != null && droneState.isFlying()) {

ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java

@@ -1,14 +1,19 @@
 package com.o3dr.android.client.apis;
 
 import android.os.Bundle;
+import android.support.annotation.IntDef;
 
 import com.o3dr.android.client.Drone;
 import com.o3dr.services.android.lib.coordinate.LatLong;
 import com.o3dr.services.android.lib.drone.attribute.AttributeType;
+import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError;
+import com.o3dr.services.android.lib.drone.property.Attitude;
 import com.o3dr.services.android.lib.drone.property.Gps;
 import com.o3dr.services.android.lib.model.AbstractCommandListener;
 import com.o3dr.services.android.lib.model.action.Action;
 
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
 import java.util.concurrent.ConcurrentHashMap;
 
 import static com.o3dr.services.android.lib.drone.action.ControlActions.ACTION_DO_GUIDED_TAKEOFF;
@@ -23,7 +28,6 @@
 import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_VELOCITY_Y;
 import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_VELOCITY_Z;
 import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_YAW_CHANGE_RATE;
-import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_YAW_IS_CLOCKWISE;
 import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_YAW_IS_RELATIVE;
 import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_YAW_TARGET_ANGLE;
 
@@ -36,6 +40,13 @@
  */
 public class ControlApi extends Api {
 
+    public static final int EARTH_NED_COORDINATE_FRAME = 0;
+    public static final int VEHICLE_COORDINATE_FRAME = 1;
+
+    @IntDef({EARTH_NED_COORDINATE_FRAME, VEHICLE_COORDINATE_FRAME})
+    @Retention(RetentionPolicy.SOURCE)
+    public @interface CoordinateFrame{}
+
     private static final ConcurrentHashMap<Drone, ControlApi> apiCache = new ConcurrentHashMap<>();
     private static final Builder<ControlApi> apiBuilder = new Builder<ControlApi>() {
         @Override
@@ -113,33 +124,67 @@ public void climbTo(double altitude) {
     /**
      * Instructs the vehicle to turn to the specified target angle
      * @param targetAngle Target angle in degrees [0-360], with 0 == north.
-     * @param turnSpeed Speed during turn in degrees per second
-     * @param isClockwise True for clockwise turn, false for counter clockwise turn
+     * @param turnRate Turning rate normalized to the range [-1.0f, 1.0f]. Positive values for clockwise turns, and negative values for counter-clockwise turns.
      * @param isRelative True is the target angle is relative to the current vehicle attitude, false otherwise if it's absolute.
      * @param listener Register a callback to receive update of the command execution state.
      */
-    public void turnTo(float targetAngle, float turnSpeed, boolean isClockwise, boolean isRelative, AbstractCommandListener listener){
+    public void turnTo(float targetAngle, float turnRate, boolean isRelative, AbstractCommandListener listener){
+        if(!isWithinBounds(targetAngle, 0, 360) || !isWithinBounds(turnRate, -1.0f, 1.0f)){
+            postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener);
+            return;
+        }
+
         Bundle params = new Bundle();
         params.putFloat(EXTRA_YAW_TARGET_ANGLE, targetAngle);
-        params.putFloat(EXTRA_YAW_CHANGE_RATE, turnSpeed);
-        params.putBoolean(EXTRA_YAW_IS_CLOCKWISE, isClockwise);
+        params.putFloat(EXTRA_YAW_CHANGE_RATE, turnRate);
         params.putBoolean(EXTRA_YAW_IS_RELATIVE, isRelative);
         drone.performAsyncActionOnDroneThread(new Action(ACTION_SET_CONDITION_YAW, params), listener);
     }
 
-    /**
-     * Move the vehicle along the specified velocity vector.
-     *
-     * @param vx x velocity in meter / s
-     * @param vy y velocity in meter / s
-     * @param vz z velocity in meter / s
-     * @param listener Register a callback to receive update of the command execution state.
-     */
-    public void moveAtVelocity(float vx, float vy, float vz, AbstractCommandListener listener){
+    private void moveAtVelocity(float vx, float vy, float vz, AbstractCommandListener listener){
         Bundle params = new Bundle();
         params.putFloat(EXTRA_VELOCITY_X, vx);
         params.putFloat(EXTRA_VELOCITY_Y, vy);
         params.putFloat(EXTRA_VELOCITY_Z, vz);
         drone.performAsyncActionOnDroneThread(new Action(ACTION_SET_VELOCITY, params), listener);
     }
+
+    /**
+     * Move the vehicle along the specified velocity vector.
+     *
+     * @param referenceFrame Reference frame to use. Can be one of
+     *                       {@link #EARTH_NED_COORDINATE_FRAME},
+     *                       {@link #VEHICLE_COORDINATE_FRAME}
+     *
+     * @param vx             x velocity normalized to the range [-1.0f, 1.0f].
+     * @param vy             y velocity normalized to the range [-1.0f, 1.0f].
+     * @param vz             z velocity normalized to the range [-1.0f, 1.0f].
+     * @param listener       Register a callback to receive update of the command execution state.
+     */
+    public void moveAtVelocity(@CoordinateFrame int referenceFrame, float vx, float vy, float vz, AbstractCommandListener listener){
+        if(!isWithinBounds(vx, -1f, 1f) || !isWithinBounds(vy, -1f, 1f) || !isWithinBounds(vz, -1f, 1f)){
+            postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener);
+            return;
+        }
+
+        float projectedX = vx;
+        float projectedY = vy;
+
+        if(referenceFrame == VEHICLE_COORDINATE_FRAME) {
+            Attitude attitude = drone.getAttribute(AttributeType.ATTITUDE);
+            double attitudeInRad = Math.toRadians(attitude.getYaw());
+
+            final double cosAttitude = Math.cos(attitudeInRad);
+            final double sinAttitude = Math.sin(attitudeInRad);
+
+            projectedX = (float) (vx * cosAttitude) - (float) (vy * sinAttitude);
+            projectedY = (float) (vx * sinAttitude) + (float) (vy * cosAttitude);
+        }
+
+        moveAtVelocity(projectedX, projectedY, vz, listener);
+    }
+
+    private static boolean isWithinBounds(float value, float lowerBound, float upperBound){
+        return value <= upperBound && value >= lowerBound;
+    }
 }

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

@@ -21,7 +21,6 @@ public class ControlActions {
     public static final String ACTION_SET_CONDITION_YAW = PACKAGE_NAME + ".SET_CONDITION_YAW";
     public static final String EXTRA_YAW_TARGET_ANGLE = "extra_yaw_target_angle";
     public static final String EXTRA_YAW_CHANGE_RATE = "extra_yaw_change_rate";
-    public static final String EXTRA_YAW_IS_CLOCKWISE = "extra_yaw_is_clockwise";
     public static final String EXTRA_YAW_IS_RELATIVE = "extra_yaw_is_relative";
 
     public static final String ACTION_SET_VELOCITY = PACKAGE_NAME + ".SET_VELOCITY";

ServiceApp/build.gradle

@@ -42,7 +42,7 @@ dependencies {
 def versionMajor = 1
 def versionMinor = 4
 def versionPatch = 3
-def versionBuild = 2 //bump for dogfood builds, public betas, etc.
+def versionBuild = 3 //bump for dogfood builds, public betas, etc.
 def versionPrefix = "3DR Services v"
 
 //Log levels values

ServiceApp/src/org/droidplanner/services/android/core/MAVLink/MavLinkModes.java → ServiceApp/src/org/droidplanner/services/android/core/MAVLink/MavLinkCommands.java

@@ -3,6 +3,7 @@
 import org.droidplanner.services.android.core.drone.variables.ApmModes;
 
 import com.MAVLink.common.msg_command_long;
+import com.MAVLink.common.msg_manual_control;
 import com.MAVLink.common.msg_mission_item;
 import com.MAVLink.common.msg_set_position_target_global_int;
 import com.MAVLink.common.msg_set_mode;
@@ -13,7 +14,7 @@
 
 import org.droidplanner.services.android.core.drone.autopilot.MavLinkDrone;
 
-public class MavLinkModes {
+public class MavLinkCommands {
 
     private static final int MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2));
     private static final int MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE = ((1 << 3) | (1 << 4) | (1 << 5));
@@ -111,4 +112,28 @@ public static void setConditionYaw(MavLinkDrone drone, float targetAngle, float
 
         drone.getMavClient().sendMavMessage(msg, listener);
     }
+
+    /**
+     * API for sending manually control to the vehicle using standard joystick axes nomenclature, along with a joystick-like input device.
+     * Unused axes can be disabled and buttons are also transmit as boolean values.
+     * @see <a href="MANUAL_CONTROL">https://pixhawk.ethz.ch/mavlink/#MANUAL_CONTROL</a>
+     *
+     * @param drone
+     * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle.
+     * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle.
+     * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle.
+     * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle.
+     * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1.
+     * @param listener
+     */
+    public static void sendManualControl(MavLinkDrone drone, short x, short y, short z, short r, int buttons, ICommandListener listener){
+        msg_manual_control msg = new msg_manual_control();
+        msg.target = drone.getSysid();
+        msg.x = x;
+        msg.y = y;
+        msg.z = z;
+        msg.r = r;
+        msg.buttons = buttons;
+        drone.getMavClient().sendMavMessage(msg, listener);
+    }
 }

ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java

@@ -47,7 +47,7 @@
 import com.o3dr.services.android.lib.model.action.Action;
 
 import org.droidplanner.services.android.core.MAVLink.MAVLinkStreams;
-import org.droidplanner.services.android.core.MAVLink.MavLinkModes;
+import org.droidplanner.services.android.core.MAVLink.MavLinkCommands;
 import org.droidplanner.services.android.core.MAVLink.MavLinkParameters;
 import org.droidplanner.services.android.core.MAVLink.WaypointManager;
 import org.droidplanner.services.android.core.MAVLink.command.doCmd.MavLinkDoCmds;
@@ -378,19 +378,28 @@ public boolean executeAsyncAction(Action action, final ICommandListener listener
                 CommonApiUtils.setGuidedAltitude(this, guidedAltitude);
                 return true;
 
-            case ControlActions.ACTION_SET_CONDITION_YAW:
-                final float targetAngle = data.getFloat(ControlActions.EXTRA_YAW_TARGET_ANGLE);
-                final float yawRate = data.getFloat(ControlActions.EXTRA_YAW_CHANGE_RATE);
-                final boolean isClockwise = data.getBoolean(ControlActions.EXTRA_YAW_IS_CLOCKWISE);
-                final boolean isRelative = data.getBoolean(ControlActions.EXTRA_YAW_IS_RELATIVE);
-                MavLinkModes.setConditionYaw(this, targetAngle, yawRate, isClockwise, isRelative, listener);
-                return true;
-
             case ControlActions.ACTION_SET_VELOCITY:
-                final float xVel = data.getFloat(ControlActions.EXTRA_VELOCITY_X);
-                final float yVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Y);
-                final float zVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Z);
-                MavLinkModes.setVelocityInLocalFrame(this, xVel, yVel, zVel, listener);
+                //Retrieve the normalized values
+                final float normalizedXVel = data.getFloat(ControlActions.EXTRA_VELOCITY_X);
+                final float normalizedYVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Y);
+                final float normalizedZVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Z);
+
+                //Retrieve the speed parameters.
+                final float defaultSpeed = 5; //m/s
+
+                //Retrieve the horizontal speed value
+                final Parameter horizSpeedParam = parameters.getParameter("WPNAV_SPEED");
+                final double horizontalSpeed = horizSpeedParam == null ? defaultSpeed : horizSpeedParam.value / 100;
+
+                //Retrieve the vertical speed value.
+                String vertSpeedParamName = normalizedZVel >= 0 ? "WPNAV_SPEED_UP" : "WPNAV_SPEED_DN";
+                final Parameter vertSpeedParam = parameters.getParameter(vertSpeedParamName);
+                final double verticalSpeed = vertSpeedParam == null ? defaultSpeed : vertSpeedParam.value / 100;
+
+                MavLinkCommands.setVelocityInLocalFrame(this, (float) (normalizedXVel * horizontalSpeed),
+                        (float) (normalizedYVel * horizontalSpeed),
+                        (float) (normalizedZVel * verticalSpeed),
+                        listener);
                 return true;
 
             //PARAMETER ACTIONS

ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java

@@ -16,6 +16,7 @@
 import com.MAVLink.common.msg_vibration;
 import com.MAVLink.enums.MAV_SYS_STATUS_SENSOR;
 import com.o3dr.services.android.lib.coordinate.LatLong;
+import com.o3dr.services.android.lib.drone.action.ControlActions;
 import com.o3dr.services.android.lib.drone.action.StateActions;
 import com.o3dr.services.android.lib.drone.attribute.AttributeEvent;
 import com.o3dr.services.android.lib.drone.attribute.AttributeType;
@@ -35,15 +36,18 @@
 import com.o3dr.services.android.lib.util.MathUtils;
 
 import org.droidplanner.services.android.core.MAVLink.MAVLinkStreams;
+import org.droidplanner.services.android.core.MAVLink.MavLinkCommands;
 import org.droidplanner.services.android.core.drone.DroneEvents;
 import org.droidplanner.services.android.core.drone.DroneInterfaces;
 import org.droidplanner.services.android.core.drone.autopilot.MavLinkDrone;
 import org.droidplanner.services.android.core.drone.variables.State;
 import org.droidplanner.services.android.core.drone.variables.StreamRates;
 import org.droidplanner.services.android.core.drone.variables.Type;
 import org.droidplanner.services.android.core.model.AutopilotWarningParser;
+import org.droidplanner.services.android.core.parameters.Parameter;
 import org.droidplanner.services.android.utils.CommonApiUtils;
 import org.droidplanner.services.android.utils.video.VideoManager;
+import org.droidplanner.services.android.core.drone.profiles.Parameters;
 
 /**
  * Base drone implementation.
@@ -179,6 +183,40 @@ public boolean executeAsyncAction(Action action, ICommandListener listener) {
                 CommonApiUtils.changeVehicleMode(this, newMode, listener);
                 return true;
 
+            //CONTROL ACTIONS
+            case ControlActions.ACTION_SET_CONDITION_YAW:
+                //Retrieve the yaw turn speed.
+                float turnSpeed = 2; //default turn speed.
+
+                final Parameters parameters = getParameters();
+                if(parameters != null){
+                    Parameter turnSpeedParam = parameters.getParameter("ACRO_YAW_P");
+                    if(turnSpeedParam != null){
+                        turnSpeed = (float) turnSpeedParam.value;
+                    }
+                }
+
+                final float targetAngle = data.getFloat(ControlActions.EXTRA_YAW_TARGET_ANGLE);
+                final float yawRate = data.getFloat(ControlActions.EXTRA_YAW_CHANGE_RATE);
+                final boolean isClockwise = yawRate >= 0;
+                final boolean isRelative = data.getBoolean(ControlActions.EXTRA_YAW_IS_RELATIVE);
+
+                MavLinkCommands.setConditionYaw(this, targetAngle, Math.abs(yawRate) * turnSpeed, isClockwise, isRelative, listener);
+                return true;
+
+            case ControlActions.ACTION_SET_VELOCITY:
+                final float xAxis = data.getFloat(ControlActions.EXTRA_VELOCITY_X);
+                final short x = (short) (xAxis * 1000);
+
+                final float yAxis = data.getFloat(ControlActions.EXTRA_VELOCITY_Y);
+                final short y = (short) (yAxis * 1000);
+
+                final float zAxis = data.getFloat(ControlActions.EXTRA_VELOCITY_Z);
+                final short z = (short) (zAxis * 1000);
+
+                MavLinkCommands.sendManualControl(this, x, y, z, (short) 0, 0, listener);
+                return true;
+
             default:
                 CommonApiUtils.postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener);
                 return true;

ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/Parameters.java

@@ -1,6 +1,7 @@
 package org.droidplanner.services.android.core.drone.profiles;
 
 import android.os.Handler;
+import android.text.TextUtils;
 
 import com.MAVLink.Messages.MAVLinkMessage;
 import com.MAVLink.common.msg_param_value;
@@ -154,7 +155,7 @@ public void readParameter(String name) {
     }
 
     public Parameter getParameter(String name) {
-        if (name == null || name.length() == 0)
+        if (TextUtils.isEmpty(name))
             return null;
 
         return parameters.get(name.toLowerCase(Locale.US));