diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index 1f2578be3f..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 = "beta2" + VERSION_SUFFIX = "release" 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/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()) { 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..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 @@ -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 apiCache = new ConcurrentHashMap<>(); private static final Builder apiBuilder = new Builder() { @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; + } } 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/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/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..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 @@ -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 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..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 @@ -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; @@ -42,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. @@ -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; 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/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() { 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 { 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..b9e7fe1049 --- /dev/null +++ b/ServiceApp/test/java/com/o3dr/android/client/apis/ControlApiTest.java @@ -0,0 +1,104 @@ +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, 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}}); + } + + /** + * 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); + 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, params[0], params[1], params[2], null); + + Assert.assertTrue(mockDrone.getAsyncAction().getType().equals(ACTION_SET_VELOCITY)); + + 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 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); + } + + +}