From efdbbde87f5594c988c49736a1881f51bb9bc36e Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Tue, 20 Oct 2015 18:55:44 -0700 Subject: [PATCH 1/9] Added reference frame parameter to the move at velocity method. --- .../o3dr/android/client/apis/ControlApi.java | 48 +++++++++++++++---- 1 file changed, 39 insertions(+), 9 deletions(-) diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java index 972ce1eb61..b60ec867de 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java @@ -1,14 +1,18 @@ 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.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; @@ -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({}) + @Retention(RetentionPolicy.SOURCE) + public @interface CoordinateFrame{} + private static final ConcurrentHashMap apiCache = new ConcurrentHashMap<>(); private static final Builder apiBuilder = new Builder() { @Override @@ -127,19 +138,38 @@ public void turnTo(float targetAngle, float turnSpeed, boolean isClockwise, bool 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 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(@CoordinateFrame int referenceFrame, float vx, float vy, float vz, AbstractCommandListener listener){ + float projectedX = vx; + float projectedY = vy; + + if(referenceFrame == VEHICLE_COORDINATE_FRAME) { + Attitude attitude = drone.getAttribute(AttributeType.ATTITUDE); + double attitudeInRad = Math.toRadians(attitude.getYaw()); + + projectedX = (float) (vy * Math.cos(attitudeInRad)) - (float) (vx * Math.sin(attitudeInRad)); + projectedY = (float) (vy * Math.sin(attitudeInRad)) + (float) (vx * Math.cos(attitudeInRad)); + } + + moveAtVelocity(projectedX, projectedY, vz, listener); + } } From 8da5a5dbcc0f2839fdf03a775e30ad30cb91a638 Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Wed, 21 Oct 2015 12:35:24 -0700 Subject: [PATCH 2/9] bug fix. --- .../src/main/java/com/o3dr/android/client/apis/ControlApi.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java index b60ec867de..82cc362017 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java @@ -43,7 +43,7 @@ public class ControlApi extends Api { public static final int EARTH_NED_COORDINATE_FRAME = 0; public static final int VEHICLE_COORDINATE_FRAME = 1; - @IntDef({}) + @IntDef({EARTH_NED_COORDINATE_FRAME, VEHICLE_COORDINATE_FRAME}) @Retention(RetentionPolicy.SOURCE) public @interface CoordinateFrame{} From a4d41067c2d5d79bb4eb39222ad7f229672270d4 Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Thu, 22 Oct 2015 04:04:29 -0700 Subject: [PATCH 3/9] normalized the speed parameters for the `turnTo` and `moveAtVelocity` control apis. added `manual_control` support for generic mavlink autopilots. --- ClientLib/build.gradle | 2 +- .../o3dr/android/client/apis/ControlApi.java | 30 +++++++++++++------ .../lib/drone/action/ControlActions.java | 1 - ...MavLinkModes.java => MavLinkCommands.java} | 27 ++++++++++++++++- .../core/drone/autopilot/apm/ArduPilot.java | 25 ++++++++-------- .../generic/GenericMavLinkDrone.java | 28 +++++++++++++++++ .../core/drone/variables/GuidedPoint.java | 10 +++---- .../android/core/drone/variables/State.java | 4 +-- 8 files changed, 95 insertions(+), 32 deletions(-) rename ServiceApp/src/org/droidplanner/services/android/core/MAVLink/{MavLinkModes.java => MavLinkCommands.java} (73%) diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index 1f2578be3f..0421f76c29 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -5,7 +5,7 @@ ext { VERSION_MINOR = 6 VERSION_PATCH = 8 VERSION_BUILD = 0 - VERSION_SUFFIX = "beta2" + VERSION_SUFFIX = "beta3" PUBLISH_ARTIFACT_ID = 'dronekit-android' PUBLISH_VERSION = generateVersionName("", VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH, VERSION_SUFFIX) diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java index 82cc362017..5d488ccfb3 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java @@ -6,6 +6,7 @@ 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; @@ -27,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; @@ -124,16 +124,19 @@ 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); } @@ -153,12 +156,17 @@ private void moveAtVelocity(float vx, float vy, float vz, AbstractCommandListene * {@link #EARTH_NED_COORDINATE_FRAME}, * {@link #VEHICLE_COORDINATE_FRAME} * - * @param vx x velocity in meter / s - * @param vy y velocity in meter / s - * @param vz z velocity in meter / s + * @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; @@ -172,4 +180,8 @@ public void moveAtVelocity(@CoordinateFrame int referenceFrame, float vx, float moveAtVelocity(projectedX, projectedY, vz, listener); } + + private static boolean isWithinBounds(float value, float lowerBound, float upperBound){ + return value <= upperBound && value >= lowerBound; + } } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/ControlActions.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/ControlActions.java index c3dd372293..06b1b69543 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/ControlActions.java +++ b/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"; diff --git a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/MavLinkModes.java b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/MavLinkCommands.java similarity index 73% rename from ServiceApp/src/org/droidplanner/services/android/core/MAVLink/MavLinkModes.java rename to ServiceApp/src/org/droidplanner/services/android/core/MAVLink/MavLinkCommands.java index f2abd8c91a..1e04cf6160 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/MavLinkModes.java +++ b/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 https://pixhawk.ethz.ch/mavlink/#MANUAL_CONTROL + * + * @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); + } } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java index 910966d05e..7f642f4b33 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java +++ b/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,18 @@ 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); + //TODO: For testing, the velocity will be capped at 5m/s. For release, retrieve and use the WPNAV_SPEED* parameters values + final float maxSpeed = 5; //m/s + + 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); + + MavLinkCommands.setVelocityInLocalFrame(this, normalizedXVel * maxSpeed, + normalizedYVel * maxSpeed, + normalizedZVel * maxSpeed, + listener); return true; //PARAMETER ACTIONS diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java index 9c167a6750..f18037cb49 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java +++ b/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,6 +36,7 @@ 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; @@ -179,6 +181,32 @@ public boolean executeAsyncAction(Action action, ICommandListener listener) { CommonApiUtils.changeVehicleMode(this, newMode, listener); return true; + //CONTROL ACTIONS + case ControlActions.ACTION_SET_CONDITION_YAW: + //TODO: for testing, cap the turning speed at 2. For release, use the value of the ACRO_YAW_P parameter. + final float turnSpeed = 2; + + 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; diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/GuidedPoint.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/GuidedPoint.java index 4c643b71cf..4701bfbc90 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/GuidedPoint.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/GuidedPoint.java @@ -11,7 +11,7 @@ import com.o3dr.services.android.lib.model.ICommandListener; import com.o3dr.services.android.lib.model.SimpleCommandListener; -import org.droidplanner.services.android.core.MAVLink.MavLinkModes; +import org.droidplanner.services.android.core.MAVLink.MavLinkCommands; import org.droidplanner.services.android.core.MAVLink.MavLinkTakeoff; import org.droidplanner.services.android.core.drone.DroneInterfaces.DroneEventsType; import org.droidplanner.services.android.core.drone.DroneInterfaces.OnDroneListener; @@ -173,11 +173,11 @@ public void newGuidedCoord(LatLong coord) { } public void newGuidedPosition(double latitude, double longitude, double altitude) { - MavLinkModes.sendGuidedPosition(myDrone, latitude, longitude, altitude); + MavLinkCommands.sendGuidedPosition(myDrone, latitude, longitude, altitude); } public void newGuidedVelocity(double xVel, double yVel, double zVel) { - MavLinkModes.sendGuidedVelocity(myDrone, xVel, yVel, zVel); + MavLinkCommands.sendGuidedVelocity(myDrone, xVel, yVel, zVel); } public void newGuidedCoordAndVelocity(LatLong coord, double xVel, double yVel, double zVel) { @@ -317,7 +317,7 @@ private void sendGuidedPoint() { public static void forceSendGuidedPoint(MavLinkDrone drone, LatLong coord, double altitudeInMeters) { drone.notifyDroneEvent(DroneEventsType.GUIDEDPOINT); if (coord != null) { - MavLinkModes.setGuidedMode(drone, coord.getLatitude(), coord.getLongitude(), altitudeInMeters); + MavLinkCommands.setGuidedMode(drone, coord.getLatitude(), coord.getLongitude(), altitudeInMeters); } } @@ -325,7 +325,7 @@ public static void forceSendGuidedPointAndVelocity(MavLinkDrone drone, LatLong c double xVel, double yVel, double zVel) { drone.notifyDroneEvent(DroneEventsType.GUIDEDPOINT); if (coord != null) { - MavLinkModes.sendGuidedPositionAndVelocity(drone, coord.getLatitude(), coord.getLongitude(), altitudeInMeters, xVel, + MavLinkCommands.sendGuidedPositionAndVelocity(drone, coord.getLatitude(), coord.getLongitude(), altitudeInMeters, xVel, yVel, zVel); } } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/State.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/State.java index 4061501165..5607abc5e4 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/State.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/State.java @@ -9,7 +9,7 @@ import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.model.ICommandListener; -import org.droidplanner.services.android.core.MAVLink.MavLinkModes; +import org.droidplanner.services.android.core.MAVLink.MavLinkCommands; import org.droidplanner.services.android.core.drone.DroneInterfaces.DroneEventsType; import org.droidplanner.services.android.core.drone.DroneVariable; import org.droidplanner.services.android.core.model.AutopilotWarningParser; @@ -138,7 +138,7 @@ public void run() { } if (ApmModes.isValid(mode)) { - MavLinkModes.changeFlightMode(myDrone, mode, listener); + MavLinkCommands.changeFlightMode(myDrone, mode, listener); } else { if (listener != null) { handler.post(new Runnable() { From 2e03e93af735bbe891c0a9331d7a1eb500d69295 Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Thu, 22 Oct 2015 13:18:06 -0700 Subject: [PATCH 4/9] javadocs update. --- .../src/main/java/com/o3dr/android/client/Drone.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ClientLib/src/main/java/com/o3dr/android/client/Drone.java b/ClientLib/src/main/java/com/o3dr/android/client/Drone.java index 02b8522122..569e141729 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/Drone.java +++ b/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()) { From be71d66c9ffaa243a0d27d1148bdfff6120c4ca2 Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Fri, 23 Oct 2015 15:27:32 -0700 Subject: [PATCH 5/9] Removed testing logic to set the control api speed. --- .../core/drone/autopilot/apm/ArduPilot.java | 22 ++++++++++++++----- .../generic/GenericMavLinkDrone.java | 14 ++++++++++-- .../core/drone/profiles/Parameters.java | 3 ++- .../android/core/parameters/Parameter.java | 6 ++--- 4 files changed, 33 insertions(+), 12 deletions(-) diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java index 7f642f4b33..2e911c8817 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/ArduPilot.java @@ -379,16 +379,26 @@ public boolean executeAsyncAction(Action action, final ICommandListener listener return true; case ControlActions.ACTION_SET_VELOCITY: - //TODO: For testing, the velocity will be capped at 5m/s. For release, retrieve and use the WPNAV_SPEED* parameters values - final float maxSpeed = 5; //m/s - + //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); - MavLinkCommands.setVelocityInLocalFrame(this, normalizedXVel * maxSpeed, - normalizedYVel * maxSpeed, - normalizedZVel * maxSpeed, + //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; diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java index f18037cb49..abaae91805 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/generic/GenericMavLinkDrone.java @@ -44,8 +44,10 @@ 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. @@ -183,8 +185,16 @@ public boolean executeAsyncAction(Action action, ICommandListener listener) { //CONTROL ACTIONS case ControlActions.ACTION_SET_CONDITION_YAW: - //TODO: for testing, cap the turning speed at 2. For release, use the value of the ACRO_YAW_P parameter. - final float turnSpeed = 2; + //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); diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/Parameters.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/Parameters.java index 808dcfa1c7..ef4cadaa34 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/Parameters.java +++ b/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)); diff --git a/ServiceApp/src/org/droidplanner/services/android/core/parameters/Parameter.java b/ServiceApp/src/org/droidplanner/services/android/core/parameters/Parameter.java index 1664691819..5ed1d33a21 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/parameters/Parameter.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/parameters/Parameter.java @@ -7,9 +7,9 @@ public class Parameter implements Comparable, Serializable { - public String name; - public double value; - public int type; + public final String name; + public final double value; + public final int type; private final static DecimalFormat format = (DecimalFormat) DecimalFormat.getInstance(); static { From eaf9a00fd629fc8a545e14ee6c576451feddc813 Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Fri, 23 Oct 2015 18:09:25 -0700 Subject: [PATCH 6/9] Added unit tests for the `ControlApi#moveAtVelocity(...)` method. versionbuild bump. --- ServiceApp/build.gradle | 2 +- .../android/client/apis/ControlApiTest.java | 101 ++++++++++++++++++ .../services/android/BasicTest.java | 5 +- .../services/android/mock/MockDrone.java | 52 +++++++++ 4 files changed, 156 insertions(+), 4 deletions(-) create mode 100644 ServiceApp/test/java/com/o3dr/android/client/apis/ControlApiTest.java create mode 100644 ServiceApp/test/java/org/droidplanner/services/android/mock/MockDrone.java diff --git a/ServiceApp/build.gradle b/ServiceApp/build.gradle index bbf3a0948b..e358044e17 100644 --- a/ServiceApp/build.gradle +++ b/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 diff --git a/ServiceApp/test/java/com/o3dr/android/client/apis/ControlApiTest.java b/ServiceApp/test/java/com/o3dr/android/client/apis/ControlApiTest.java new file mode 100644 index 0000000000..717d5a79c6 --- /dev/null +++ b/ServiceApp/test/java/com/o3dr/android/client/apis/ControlApiTest.java @@ -0,0 +1,101 @@ +package com.o3dr.android.client.apis; + +import android.content.Context; +import android.os.Bundle; +import android.util.SparseArray; + +import com.o3dr.services.android.lib.drone.attribute.AttributeType; +import com.o3dr.services.android.lib.drone.property.Attitude; +import com.o3dr.services.android.lib.model.AbstractCommandListener; +import com.o3dr.services.android.lib.model.action.Action; + +import org.droidplanner.services.android.mock.MockDrone; +import org.junit.Assert; +import org.junit.Test; +import org.junit.runner.RunWith; +import org.robolectric.Robolectric; +import org.robolectric.RobolectricTestRunner; +import org.robolectric.annotation.Config; + +import static com.o3dr.services.android.lib.drone.action.ControlActions.ACTION_SET_VELOCITY; +import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_VELOCITY_X; +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; + +/** + * Created by Fredia Huya-Kouadio on 10/23/15. + */ +@RunWith(RobolectricTestRunner.class) +@Config(emulateSdk = 18) +public class ControlApiTest { + + private static final SparseArray expectedVelocitiesPerAttitude = new SparseArray<>(); + static { + expectedVelocitiesPerAttitude.append(0, new float[]{1f, 1f, 1f}); + expectedVelocitiesPerAttitude.append(45, new float[]{0f, (float) Math.sqrt(2), 1f}); + expectedVelocitiesPerAttitude.append(90, new float[]{-1f, 1f, 1f}); + expectedVelocitiesPerAttitude.append(135, new float[]{-(float) Math.sqrt(2), 0, 1f}); + expectedVelocitiesPerAttitude.append(180, new float[]{-1f, -1f, 1f}); + expectedVelocitiesPerAttitude.append(225, new float[]{0f, -(float) Math.sqrt(2), 1f}); + expectedVelocitiesPerAttitude.append(270, new float[]{1f, -1f, 1f}); + expectedVelocitiesPerAttitude.append(315, new float[]{(float) Math.sqrt(2), 0, 1f}); + expectedVelocitiesPerAttitude.append(360, new float[]{1f, 1f, 1f}); + } + + /** + * Tests the ControlApi#moveAtVelocity(...) method. + * Ensures the method correctly interpret its given parameters. + * + * @throws Exception + */ + @Test + public void testMoveAtVelocity() throws Exception { + final Context context = Robolectric.getShadowApplication().getApplicationContext(); + final MockDrone mockDrone = new MockDrone(context) { + @Override + public boolean performAsyncActionOnDroneThread(Action action, AbstractCommandListener listener) { + this.asyncAction = action; + return true; + } + + }; + + final ControlApi controlApi = ControlApi.getApi(mockDrone); + + //Test with the EARTH NED coordinate frame. What goes in should be what comes out. + final int testCount = 100; + for(int i= 0; i < testCount; i++){ + final float randomX = (float) ((Math.random() * 2) - 1f); + final float randomY =(float) ((Math.random() * 2) - 1f); + final float randomZ = (float) ((Math.random() * 2) - 1f); + + controlApi.moveAtVelocity(ControlApi.EARTH_NED_COORDINATE_FRAME, randomX, randomY, randomZ, null); + + Assert.assertTrue(mockDrone.getAsyncAction().getType().equals(ACTION_SET_VELOCITY)); + + Bundle params = mockDrone.getAsyncAction().getData(); + Assert.assertEquals(params.getFloat(EXTRA_VELOCITY_X), randomX, 0.001); + Assert.assertEquals(params.getFloat(EXTRA_VELOCITY_Y), randomY, 0.001); + Assert.assertEquals(params.getFloat(EXTRA_VELOCITY_Z), randomZ, 0.001); + } + + //Test with the VEHICLE coordinate frame. The output is dependent on the vehicle attitude data. + final Attitude attitude = new Attitude(); + final int expectedValuesCount = expectedVelocitiesPerAttitude.size(); + for(int i = 0; i < expectedValuesCount; i++) { + final int yaw = expectedVelocitiesPerAttitude.keyAt(i); + attitude.setYaw(yaw); + mockDrone.setAttribute(AttributeType.ATTITUDE, attitude); + + controlApi.moveAtVelocity(ControlApi.VEHICLE_COORDINATE_FRAME, 1, 1, 1, null); + + Assert.assertTrue(mockDrone.getAsyncAction().getType().equals(ACTION_SET_VELOCITY)); + + final float[] expectedValues = expectedVelocitiesPerAttitude.valueAt(i); + Bundle params = mockDrone.getAsyncAction().getData(); + Assert.assertEquals("Invalid x velocity for attitude = " + yaw, params.getFloat(EXTRA_VELOCITY_X), expectedValues[0], 0.001); + Assert.assertEquals("Invalid y velocity for attitude = " + yaw, params.getFloat(EXTRA_VELOCITY_Y), expectedValues[1], 0.001); + Assert.assertEquals("Invalid z velocity for attitude = " + yaw, params.getFloat(EXTRA_VELOCITY_Z), expectedValues[2], 0.001); + } + } +} \ No newline at end of file diff --git a/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java b/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java index 74cfa3ad56..3d4f164819 100644 --- a/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java +++ b/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java @@ -3,7 +3,6 @@ import android.content.Context; import android.os.Bundle; import android.os.Handler; -import android.os.SystemClock; import com.MAVLink.MAVLinkPacket; import com.MAVLink.Messages.MAVLinkMessage; @@ -11,13 +10,13 @@ import com.MAVLink.enums.MAV_CMD; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; +import org.droidplanner.services.android.communication.service.MAVLinkClient; import org.droidplanner.services.android.core.MAVLink.MAVLinkStreams; import org.droidplanner.services.android.core.MAVLink.MavLinkArm; -import org.droidplanner.services.android.core.drone.autopilot.apm.ArduCopter; import org.droidplanner.services.android.core.drone.DroneInterfaces; import org.droidplanner.services.android.core.drone.LogMessageListener; import org.droidplanner.services.android.core.drone.autopilot.MavLinkDrone; -import org.droidplanner.services.android.communication.service.MAVLinkClient; +import org.droidplanner.services.android.core.drone.autopilot.apm.ArduCopter; import org.droidplanner.services.android.mock.MockMavLinkServiceAPI; import org.droidplanner.services.android.utils.AndroidApWarningParser; import org.droidplanner.services.android.utils.prefs.DroidPlannerPrefs; diff --git a/ServiceApp/test/java/org/droidplanner/services/android/mock/MockDrone.java b/ServiceApp/test/java/org/droidplanner/services/android/mock/MockDrone.java new file mode 100644 index 0000000000..7e03e2dd93 --- /dev/null +++ b/ServiceApp/test/java/org/droidplanner/services/android/mock/MockDrone.java @@ -0,0 +1,52 @@ +package org.droidplanner.services.android.mock; + +import android.content.Context; +import android.os.Parcelable; + +import com.o3dr.android.client.Drone; +import com.o3dr.services.android.lib.drone.attribute.AttributeType; +import com.o3dr.services.android.lib.drone.property.Attitude; +import com.o3dr.services.android.lib.drone.property.DroneAttribute; +import com.o3dr.services.android.lib.model.action.Action; + +import java.util.HashMap; + +/** + * Mock implementation for the Drone object. + * Created by Fredia Huya-Kouadio on 10/23/15. + */ +public abstract class MockDrone extends Drone { + + private final HashMap mockAttributes = new HashMap<>(); + + protected Action asyncAction; + protected Action syncAction; + + /** + * Creates a Drone instance. + * + * @param context Application context + */ + public MockDrone(Context context) { + super(context); + } + + public Action getAsyncAction() { + return asyncAction; + } + + public Action getSyncAction() { + return syncAction; + } + + public void setAttribute(String attributeType, DroneAttribute attribute){ + mockAttributes.put(attributeType, attribute); + } + + @Override + public T getAttribute(String type) { + return (T) mockAttributes.get(type); + } + + +} From a19d13ba722326bb459331cd3f2bbd8ab3357943 Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Fri, 23 Oct 2015 18:10:37 -0700 Subject: [PATCH 7/9] code cleanup. client library beta version bump. --- ClientLib/build.gradle | 2 +- .../main/java/com/o3dr/android/client/apis/ControlApi.java | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index 0421f76c29..090efeab93 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -5,7 +5,7 @@ ext { VERSION_MINOR = 6 VERSION_PATCH = 8 VERSION_BUILD = 0 - VERSION_SUFFIX = "beta3" + VERSION_SUFFIX = "beta4" PUBLISH_ARTIFACT_ID = 'dronekit-android' PUBLISH_VERSION = generateVersionName("", VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH, VERSION_SUFFIX) diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java index 5d488ccfb3..95befe8775 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java @@ -174,8 +174,11 @@ public void moveAtVelocity(@CoordinateFrame int referenceFrame, float vx, float Attitude attitude = drone.getAttribute(AttributeType.ATTITUDE); double attitudeInRad = Math.toRadians(attitude.getYaw()); - projectedX = (float) (vy * Math.cos(attitudeInRad)) - (float) (vx * Math.sin(attitudeInRad)); - projectedY = (float) (vy * Math.sin(attitudeInRad)) + (float) (vx * Math.cos(attitudeInRad)); + final double cosAttitude = Math.cos(attitudeInRad); + final double sinAttitude = Math.sin(attitudeInRad); + + projectedX = (float) (vy * cosAttitude) - (float) (vx * sinAttitude); + projectedY = (float) (vy * sinAttitude) + (float) (vx * cosAttitude); } moveAtVelocity(projectedX, projectedY, vz, listener); From a7763b8ef4815ad68113c28f469b505b4d6429ea Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Fri, 23 Oct 2015 19:03:51 -0700 Subject: [PATCH 8/9] bug fix. --- ClientLib/build.gradle | 2 +- .../o3dr/android/client/apis/ControlApi.java | 4 +-- .../android/client/apis/ControlApiTest.java | 35 ++++++++++--------- 3 files changed, 22 insertions(+), 19 deletions(-) diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index 090efeab93..81a077b4be 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -5,7 +5,7 @@ ext { VERSION_MINOR = 6 VERSION_PATCH = 8 VERSION_BUILD = 0 - VERSION_SUFFIX = "beta4" + VERSION_SUFFIX = "beta5" PUBLISH_ARTIFACT_ID = 'dronekit-android' PUBLISH_VERSION = generateVersionName("", VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH, VERSION_SUFFIX) diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java index 95befe8775..c2539e49da 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/ControlApi.java @@ -177,8 +177,8 @@ public void moveAtVelocity(@CoordinateFrame int referenceFrame, float vx, float final double cosAttitude = Math.cos(attitudeInRad); final double sinAttitude = Math.sin(attitudeInRad); - projectedX = (float) (vy * cosAttitude) - (float) (vx * sinAttitude); - projectedY = (float) (vy * sinAttitude) + (float) (vx * cosAttitude); + projectedX = (float) (vx * cosAttitude) - (float) (vy * sinAttitude); + projectedY = (float) (vx * sinAttitude) + (float) (vy * cosAttitude); } moveAtVelocity(projectedX, projectedY, vz, listener); diff --git a/ServiceApp/test/java/com/o3dr/android/client/apis/ControlApiTest.java b/ServiceApp/test/java/com/o3dr/android/client/apis/ControlApiTest.java index 717d5a79c6..b9e7fe1049 100644 --- a/ServiceApp/test/java/com/o3dr/android/client/apis/ControlApiTest.java +++ b/ServiceApp/test/java/com/o3dr/android/client/apis/ControlApiTest.java @@ -29,17 +29,17 @@ @Config(emulateSdk = 18) public class ControlApiTest { - private static final SparseArray expectedVelocitiesPerAttitude = new SparseArray<>(); + private static final SparseArray expectedVelocitiesPerAttitude = new SparseArray<>(); static { - expectedVelocitiesPerAttitude.append(0, new float[]{1f, 1f, 1f}); - expectedVelocitiesPerAttitude.append(45, new float[]{0f, (float) Math.sqrt(2), 1f}); - expectedVelocitiesPerAttitude.append(90, new float[]{-1f, 1f, 1f}); - expectedVelocitiesPerAttitude.append(135, new float[]{-(float) Math.sqrt(2), 0, 1f}); - expectedVelocitiesPerAttitude.append(180, new float[]{-1f, -1f, 1f}); - expectedVelocitiesPerAttitude.append(225, new float[]{0f, -(float) Math.sqrt(2), 1f}); - expectedVelocitiesPerAttitude.append(270, new float[]{1f, -1f, 1f}); - expectedVelocitiesPerAttitude.append(315, new float[]{(float) Math.sqrt(2), 0, 1f}); - expectedVelocitiesPerAttitude.append(360, new float[]{1f, 1f, 1f}); + expectedVelocitiesPerAttitude.append(0, new float[][]{{1f, 0f, 1f}, {1f, 0f, 1f}}); + expectedVelocitiesPerAttitude.append(45, new float[][]{{1f, 1f, 1f}, {0f, (float) Math.sqrt(2), 1f}}); + expectedVelocitiesPerAttitude.append(90, new float[][]{{1f, 1f, 1f},{-1f, 1f, 1f}}); + expectedVelocitiesPerAttitude.append(135, new float[][]{{1f, 1f, 1f}, {-(float) Math.sqrt(2), 0, 1f}}); + expectedVelocitiesPerAttitude.append(180, new float[][]{{1f, 1f, 1f}, {-1f, -1f, 1f}}); + expectedVelocitiesPerAttitude.append(225, new float[][]{{1f, 1f, 1f}, {0f, -(float) Math.sqrt(2), 1f}}); + expectedVelocitiesPerAttitude.append(270, new float[][]{{1f, 1f, 1f},{1f, -1f, 1f}}); + expectedVelocitiesPerAttitude.append(315, new float[][]{{1f, 1f, 1f}, {(float) Math.sqrt(2), 0, 1f}}); + expectedVelocitiesPerAttitude.append(360, new float[][]{{1f, 1f, 1f},{1f, 1f, 1f}}); } /** @@ -84,18 +84,21 @@ public boolean performAsyncActionOnDroneThread(Action action, AbstractCommandLis final int expectedValuesCount = expectedVelocitiesPerAttitude.size(); for(int i = 0; i < expectedValuesCount; i++) { final int yaw = expectedVelocitiesPerAttitude.keyAt(i); + final float[][] paramsAndResults = expectedVelocitiesPerAttitude.valueAt(i); + final float[] params = paramsAndResults[0]; + attitude.setYaw(yaw); mockDrone.setAttribute(AttributeType.ATTITUDE, attitude); - controlApi.moveAtVelocity(ControlApi.VEHICLE_COORDINATE_FRAME, 1, 1, 1, null); + controlApi.moveAtVelocity(ControlApi.VEHICLE_COORDINATE_FRAME, params[0], params[1], params[2], null); Assert.assertTrue(mockDrone.getAsyncAction().getType().equals(ACTION_SET_VELOCITY)); - final float[] expectedValues = expectedVelocitiesPerAttitude.valueAt(i); - Bundle params = mockDrone.getAsyncAction().getData(); - Assert.assertEquals("Invalid x velocity for attitude = " + yaw, params.getFloat(EXTRA_VELOCITY_X), expectedValues[0], 0.001); - Assert.assertEquals("Invalid y velocity for attitude = " + yaw, params.getFloat(EXTRA_VELOCITY_Y), expectedValues[1], 0.001); - Assert.assertEquals("Invalid z velocity for attitude = " + yaw, params.getFloat(EXTRA_VELOCITY_Z), expectedValues[2], 0.001); + final float[] expectedValues = paramsAndResults[1]; + Bundle data = mockDrone.getAsyncAction().getData(); + Assert.assertEquals("Invalid x velocity for attitude = " + yaw, data.getFloat(EXTRA_VELOCITY_X), expectedValues[0], 0.001); + Assert.assertEquals("Invalid y velocity for attitude = " + yaw, data.getFloat(EXTRA_VELOCITY_Y), expectedValues[1], 0.001); + Assert.assertEquals("Invalid z velocity for attitude = " + yaw, data.getFloat(EXTRA_VELOCITY_Z), expectedValues[2], 0.001); } } } \ No newline at end of file From 5955028b77e904bcdcc90892b0b77e03899dd6fa Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Fri, 23 Oct 2015 19:30:46 -0700 Subject: [PATCH 9/9] bump client lib version. --- ClientLib/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index 81a077b4be..e5f580620f 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -5,7 +5,7 @@ ext { VERSION_MINOR = 6 VERSION_PATCH = 8 VERSION_BUILD = 0 - VERSION_SUFFIX = "beta5" + VERSION_SUFFIX = "release" PUBLISH_ARTIFACT_ID = 'dronekit-android' PUBLISH_VERSION = generateVersionName("", VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH, VERSION_SUFFIX)