Skip to content

Commit 1081549

Browse files
m4gr3dm4gr3d
committed
Merge pull request #317 from dronekit/improve_control_api
Control Api updates.
2 parents ff75bfa + 5955028 commit 1081549

File tree

15 files changed

+325
-47
lines changed

15 files changed

+325
-47
lines changed

‎ClientLib/build.gradle

+1-1
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@ ext {
55
VERSION_MINOR = 6
66
VERSION_PATCH = 8
77
VERSION_BUILD = 0
8-
VERSION_SUFFIX = "beta2"
8+
VERSION_SUFFIX = "release"
99

1010
PUBLISH_ARTIFACT_ID = 'dronekit-android'
1111
PUBLISH_VERSION = generateVersionName("", VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH, VERSION_SUFFIX)

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

+7-1
Original file line numberDiff line numberDiff line change
@@ -233,17 +233,23 @@ public void post(Runnable action) {
233233
handler.post(action);
234234
}
235235

236+
/**
237+
* Reset the vehicle flight timer.
238+
*/
236239
public void resetFlightTimer() {
237240
elapsedFlightTime = 0;
238241
startTime = SystemClock.elapsedRealtime();
239242
}
240243

241-
public void stopTimer() {
244+
private void stopTimer() {
242245
// lets calc the final elapsed timer
243246
elapsedFlightTime += SystemClock.elapsedRealtime() - startTime;
244247
startTime = SystemClock.elapsedRealtime();
245248
}
246249

250+
/**
251+
* @return Vehicle flight time in seconds.
252+
*/
247253
public long getFlightTime() {
248254
State droneState = getAttribute(AttributeType.STATE);
249255
if (droneState != null && droneState.isFlying()) {

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

+60-15
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,19 @@
11
package com.o3dr.android.client.apis;
22

33
import android.os.Bundle;
4+
import android.support.annotation.IntDef;
45

56
import com.o3dr.android.client.Drone;
67
import com.o3dr.services.android.lib.coordinate.LatLong;
78
import com.o3dr.services.android.lib.drone.attribute.AttributeType;
9+
import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError;
10+
import com.o3dr.services.android.lib.drone.property.Attitude;
811
import com.o3dr.services.android.lib.drone.property.Gps;
912
import com.o3dr.services.android.lib.model.AbstractCommandListener;
1013
import com.o3dr.services.android.lib.model.action.Action;
1114

15+
import java.lang.annotation.Retention;
16+
import java.lang.annotation.RetentionPolicy;
1217
import java.util.concurrent.ConcurrentHashMap;
1318

1419
import static com.o3dr.services.android.lib.drone.action.ControlActions.ACTION_DO_GUIDED_TAKEOFF;
@@ -23,7 +28,6 @@
2328
import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_VELOCITY_Y;
2429
import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_VELOCITY_Z;
2530
import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_YAW_CHANGE_RATE;
26-
import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_YAW_IS_CLOCKWISE;
2731
import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_YAW_IS_RELATIVE;
2832
import static com.o3dr.services.android.lib.drone.action.ControlActions.EXTRA_YAW_TARGET_ANGLE;
2933

@@ -36,6 +40,13 @@
3640
*/
3741
public class ControlApi extends Api {
3842

43+
public static final int EARTH_NED_COORDINATE_FRAME = 0;
44+
public static final int VEHICLE_COORDINATE_FRAME = 1;
45+
46+
@IntDef({EARTH_NED_COORDINATE_FRAME, VEHICLE_COORDINATE_FRAME})
47+
@Retention(RetentionPolicy.SOURCE)
48+
public @interface CoordinateFrame{}
49+
3950
private static final ConcurrentHashMap<Drone, ControlApi> apiCache = new ConcurrentHashMap<>();
4051
private static final Builder<ControlApi> apiBuilder = new Builder<ControlApi>() {
4152
@Override
@@ -113,33 +124,67 @@ public void climbTo(double altitude) {
113124
/**
114125
* Instructs the vehicle to turn to the specified target angle
115126
* @param targetAngle Target angle in degrees [0-360], with 0 == north.
116-
* @param turnSpeed Speed during turn in degrees per second
117-
* @param isClockwise True for clockwise turn, false for counter clockwise turn
127+
* @param turnRate Turning rate normalized to the range [-1.0f, 1.0f]. Positive values for clockwise turns, and negative values for counter-clockwise turns.
118128
* @param isRelative True is the target angle is relative to the current vehicle attitude, false otherwise if it's absolute.
119129
* @param listener Register a callback to receive update of the command execution state.
120130
*/
121-
public void turnTo(float targetAngle, float turnSpeed, boolean isClockwise, boolean isRelative, AbstractCommandListener listener){
131+
public void turnTo(float targetAngle, float turnRate, boolean isRelative, AbstractCommandListener listener){
132+
if(!isWithinBounds(targetAngle, 0, 360) || !isWithinBounds(turnRate, -1.0f, 1.0f)){
133+
postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener);
134+
return;
135+
}
136+
122137
Bundle params = new Bundle();
123138
params.putFloat(EXTRA_YAW_TARGET_ANGLE, targetAngle);
124-
params.putFloat(EXTRA_YAW_CHANGE_RATE, turnSpeed);
125-
params.putBoolean(EXTRA_YAW_IS_CLOCKWISE, isClockwise);
139+
params.putFloat(EXTRA_YAW_CHANGE_RATE, turnRate);
126140
params.putBoolean(EXTRA_YAW_IS_RELATIVE, isRelative);
127141
drone.performAsyncActionOnDroneThread(new Action(ACTION_SET_CONDITION_YAW, params), listener);
128142
}
129143

130-
/**
131-
* Move the vehicle along the specified velocity vector.
132-
*
133-
* @param vx x velocity in meter / s
134-
* @param vy y velocity in meter / s
135-
* @param vz z velocity in meter / s
136-
* @param listener Register a callback to receive update of the command execution state.
137-
*/
138-
public void moveAtVelocity(float vx, float vy, float vz, AbstractCommandListener listener){
144+
private void moveAtVelocity(float vx, float vy, float vz, AbstractCommandListener listener){
139145
Bundle params = new Bundle();
140146
params.putFloat(EXTRA_VELOCITY_X, vx);
141147
params.putFloat(EXTRA_VELOCITY_Y, vy);
142148
params.putFloat(EXTRA_VELOCITY_Z, vz);
143149
drone.performAsyncActionOnDroneThread(new Action(ACTION_SET_VELOCITY, params), listener);
144150
}
151+
152+
/**
153+
* Move the vehicle along the specified velocity vector.
154+
*
155+
* @param referenceFrame Reference frame to use. Can be one of
156+
* {@link #EARTH_NED_COORDINATE_FRAME},
157+
* {@link #VEHICLE_COORDINATE_FRAME}
158+
*
159+
* @param vx x velocity normalized to the range [-1.0f, 1.0f].
160+
* @param vy y velocity normalized to the range [-1.0f, 1.0f].
161+
* @param vz z velocity normalized to the range [-1.0f, 1.0f].
162+
* @param listener Register a callback to receive update of the command execution state.
163+
*/
164+
public void moveAtVelocity(@CoordinateFrame int referenceFrame, float vx, float vy, float vz, AbstractCommandListener listener){
165+
if(!isWithinBounds(vx, -1f, 1f) || !isWithinBounds(vy, -1f, 1f) || !isWithinBounds(vz, -1f, 1f)){
166+
postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener);
167+
return;
168+
}
169+
170+
float projectedX = vx;
171+
float projectedY = vy;
172+
173+
if(referenceFrame == VEHICLE_COORDINATE_FRAME) {
174+
Attitude attitude = drone.getAttribute(AttributeType.ATTITUDE);
175+
double attitudeInRad = Math.toRadians(attitude.getYaw());
176+
177+
final double cosAttitude = Math.cos(attitudeInRad);
178+
final double sinAttitude = Math.sin(attitudeInRad);
179+
180+
projectedX = (float) (vx * cosAttitude) - (float) (vy * sinAttitude);
181+
projectedY = (float) (vx * sinAttitude) + (float) (vy * cosAttitude);
182+
}
183+
184+
moveAtVelocity(projectedX, projectedY, vz, listener);
185+
}
186+
187+
private static boolean isWithinBounds(float value, float lowerBound, float upperBound){
188+
return value <= upperBound && value >= lowerBound;
189+
}
145190
}

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

-1
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,6 @@ public class ControlActions {
2121
public static final String ACTION_SET_CONDITION_YAW = PACKAGE_NAME + ".SET_CONDITION_YAW";
2222
public static final String EXTRA_YAW_TARGET_ANGLE = "extra_yaw_target_angle";
2323
public static final String EXTRA_YAW_CHANGE_RATE = "extra_yaw_change_rate";
24-
public static final String EXTRA_YAW_IS_CLOCKWISE = "extra_yaw_is_clockwise";
2524
public static final String EXTRA_YAW_IS_RELATIVE = "extra_yaw_is_relative";
2625

2726
public static final String ACTION_SET_VELOCITY = PACKAGE_NAME + ".SET_VELOCITY";

‎ServiceApp/build.gradle

+1-1
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ dependencies {
4242
def versionMajor = 1
4343
def versionMinor = 4
4444
def versionPatch = 3
45-
def versionBuild = 2 //bump for dogfood builds, public betas, etc.
45+
def versionBuild = 3 //bump for dogfood builds, public betas, etc.
4646
def versionPrefix = "3DR Services v"
4747

4848
//Log levels values

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

+26-1
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
import org.droidplanner.services.android.core.drone.variables.ApmModes;
44

55
import com.MAVLink.common.msg_command_long;
6+
import com.MAVLink.common.msg_manual_control;
67
import com.MAVLink.common.msg_mission_item;
78
import com.MAVLink.common.msg_set_position_target_global_int;
89
import com.MAVLink.common.msg_set_mode;
@@ -13,7 +14,7 @@
1314

1415
import org.droidplanner.services.android.core.drone.autopilot.MavLinkDrone;
1516

16-
public class MavLinkModes {
17+
public class MavLinkCommands {
1718

1819
private static final int MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = ((1 << 0) | (1 << 1) | (1 << 2));
1920
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
111112

112113
drone.getMavClient().sendMavMessage(msg, listener);
113114
}
115+
116+
/**
117+
* API for sending manually control to the vehicle using standard joystick axes nomenclature, along with a joystick-like input device.
118+
* Unused axes can be disabled and buttons are also transmit as boolean values.
119+
* @see <a href="MANUAL_CONTROL">https://pixhawk.ethz.ch/mavlink/#MANUAL_CONTROL</a>
120+
*
121+
* @param drone
122+
* @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.
123+
* @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.
124+
* @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.
125+
* @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.
126+
* @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1.
127+
* @param listener
128+
*/
129+
public static void sendManualControl(MavLinkDrone drone, short x, short y, short z, short r, int buttons, ICommandListener listener){
130+
msg_manual_control msg = new msg_manual_control();
131+
msg.target = drone.getSysid();
132+
msg.x = x;
133+
msg.y = y;
134+
msg.z = z;
135+
msg.r = r;
136+
msg.buttons = buttons;
137+
drone.getMavClient().sendMavMessage(msg, listener);
138+
}
114139
}

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

+22-13
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@
4747
import com.o3dr.services.android.lib.model.action.Action;
4848

4949
import org.droidplanner.services.android.core.MAVLink.MAVLinkStreams;
50-
import org.droidplanner.services.android.core.MAVLink.MavLinkModes;
50+
import org.droidplanner.services.android.core.MAVLink.MavLinkCommands;
5151
import org.droidplanner.services.android.core.MAVLink.MavLinkParameters;
5252
import org.droidplanner.services.android.core.MAVLink.WaypointManager;
5353
import org.droidplanner.services.android.core.MAVLink.command.doCmd.MavLinkDoCmds;
@@ -378,19 +378,28 @@ public boolean executeAsyncAction(Action action, final ICommandListener listener
378378
CommonApiUtils.setGuidedAltitude(this, guidedAltitude);
379379
return true;
380380

381-
case ControlActions.ACTION_SET_CONDITION_YAW:
382-
final float targetAngle = data.getFloat(ControlActions.EXTRA_YAW_TARGET_ANGLE);
383-
final float yawRate = data.getFloat(ControlActions.EXTRA_YAW_CHANGE_RATE);
384-
final boolean isClockwise = data.getBoolean(ControlActions.EXTRA_YAW_IS_CLOCKWISE);
385-
final boolean isRelative = data.getBoolean(ControlActions.EXTRA_YAW_IS_RELATIVE);
386-
MavLinkModes.setConditionYaw(this, targetAngle, yawRate, isClockwise, isRelative, listener);
387-
return true;
388-
389381
case ControlActions.ACTION_SET_VELOCITY:
390-
final float xVel = data.getFloat(ControlActions.EXTRA_VELOCITY_X);
391-
final float yVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Y);
392-
final float zVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Z);
393-
MavLinkModes.setVelocityInLocalFrame(this, xVel, yVel, zVel, listener);
382+
//Retrieve the normalized values
383+
final float normalizedXVel = data.getFloat(ControlActions.EXTRA_VELOCITY_X);
384+
final float normalizedYVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Y);
385+
final float normalizedZVel = data.getFloat(ControlActions.EXTRA_VELOCITY_Z);
386+
387+
//Retrieve the speed parameters.
388+
final float defaultSpeed = 5; //m/s
389+
390+
//Retrieve the horizontal speed value
391+
final Parameter horizSpeedParam = parameters.getParameter("WPNAV_SPEED");
392+
final double horizontalSpeed = horizSpeedParam == null ? defaultSpeed : horizSpeedParam.value / 100;
393+
394+
//Retrieve the vertical speed value.
395+
String vertSpeedParamName = normalizedZVel >= 0 ? "WPNAV_SPEED_UP" : "WPNAV_SPEED_DN";
396+
final Parameter vertSpeedParam = parameters.getParameter(vertSpeedParamName);
397+
final double verticalSpeed = vertSpeedParam == null ? defaultSpeed : vertSpeedParam.value / 100;
398+
399+
MavLinkCommands.setVelocityInLocalFrame(this, (float) (normalizedXVel * horizontalSpeed),
400+
(float) (normalizedYVel * horizontalSpeed),
401+
(float) (normalizedZVel * verticalSpeed),
402+
listener);
394403
return true;
395404

396405
//PARAMETER ACTIONS

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

+38
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
import com.MAVLink.common.msg_vibration;
1717
import com.MAVLink.enums.MAV_SYS_STATUS_SENSOR;
1818
import com.o3dr.services.android.lib.coordinate.LatLong;
19+
import com.o3dr.services.android.lib.drone.action.ControlActions;
1920
import com.o3dr.services.android.lib.drone.action.StateActions;
2021
import com.o3dr.services.android.lib.drone.attribute.AttributeEvent;
2122
import com.o3dr.services.android.lib.drone.attribute.AttributeType;
@@ -35,15 +36,18 @@
3536
import com.o3dr.services.android.lib.util.MathUtils;
3637

3738
import org.droidplanner.services.android.core.MAVLink.MAVLinkStreams;
39+
import org.droidplanner.services.android.core.MAVLink.MavLinkCommands;
3840
import org.droidplanner.services.android.core.drone.DroneEvents;
3941
import org.droidplanner.services.android.core.drone.DroneInterfaces;
4042
import org.droidplanner.services.android.core.drone.autopilot.MavLinkDrone;
4143
import org.droidplanner.services.android.core.drone.variables.State;
4244
import org.droidplanner.services.android.core.drone.variables.StreamRates;
4345
import org.droidplanner.services.android.core.drone.variables.Type;
4446
import org.droidplanner.services.android.core.model.AutopilotWarningParser;
47+
import org.droidplanner.services.android.core.parameters.Parameter;
4548
import org.droidplanner.services.android.utils.CommonApiUtils;
4649
import org.droidplanner.services.android.utils.video.VideoManager;
50+
import org.droidplanner.services.android.core.drone.profiles.Parameters;
4751

4852
/**
4953
* Base drone implementation.
@@ -179,6 +183,40 @@ public boolean executeAsyncAction(Action action, ICommandListener listener) {
179183
CommonApiUtils.changeVehicleMode(this, newMode, listener);
180184
return true;
181185

186+
//CONTROL ACTIONS
187+
case ControlActions.ACTION_SET_CONDITION_YAW:
188+
//Retrieve the yaw turn speed.
189+
float turnSpeed = 2; //default turn speed.
190+
191+
final Parameters parameters = getParameters();
192+
if(parameters != null){
193+
Parameter turnSpeedParam = parameters.getParameter("ACRO_YAW_P");
194+
if(turnSpeedParam != null){
195+
turnSpeed = (float) turnSpeedParam.value;
196+
}
197+
}
198+
199+
final float targetAngle = data.getFloat(ControlActions.EXTRA_YAW_TARGET_ANGLE);
200+
final float yawRate = data.getFloat(ControlActions.EXTRA_YAW_CHANGE_RATE);
201+
final boolean isClockwise = yawRate >= 0;
202+
final boolean isRelative = data.getBoolean(ControlActions.EXTRA_YAW_IS_RELATIVE);
203+
204+
MavLinkCommands.setConditionYaw(this, targetAngle, Math.abs(yawRate) * turnSpeed, isClockwise, isRelative, listener);
205+
return true;
206+
207+
case ControlActions.ACTION_SET_VELOCITY:
208+
final float xAxis = data.getFloat(ControlActions.EXTRA_VELOCITY_X);
209+
final short x = (short) (xAxis * 1000);
210+
211+
final float yAxis = data.getFloat(ControlActions.EXTRA_VELOCITY_Y);
212+
final short y = (short) (yAxis * 1000);
213+
214+
final float zAxis = data.getFloat(ControlActions.EXTRA_VELOCITY_Z);
215+
final short z = (short) (zAxis * 1000);
216+
217+
MavLinkCommands.sendManualControl(this, x, y, z, (short) 0, 0, listener);
218+
return true;
219+
182220
default:
183221
CommonApiUtils.postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener);
184222
return true;

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

+2-1
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
package org.droidplanner.services.android.core.drone.profiles;
22

33
import android.os.Handler;
4+
import android.text.TextUtils;
45

56
import com.MAVLink.Messages.MAVLinkMessage;
67
import com.MAVLink.common.msg_param_value;
@@ -154,7 +155,7 @@ public void readParameter(String name) {
154155
}
155156

156157
public Parameter getParameter(String name) {
157-
if (name == null || name.length() == 0)
158+
if (TextUtils.isEmpty(name))
158159
return null;
159160

160161
return parameters.get(name.toLowerCase(Locale.US));

0 commit comments

Comments
 (0)