Skip to content

Commit

Permalink
Merge pull request #122 from DroidPlanner/feature_udp_ping
Browse files Browse the repository at this point in the history
Feature UDP Client connection
  • Loading branch information
m4gr3d committed Apr 24, 2015
2 parents 8efeab3 + 4b9270a commit b85e70d
Showing 11 changed files with 271 additions and 77 deletions.
4 changes: 2 additions & 2 deletions ClientLib/build.gradle
Original file line number Diff line number Diff line change
@@ -2,7 +2,7 @@ apply plugin: 'com.android.library'

ext {
PUBLISH_ARTIFACT_ID = 'dronekit-android'
PUBLISH_VERSION = '2.3.09'
PUBLISH_VERSION = '2.3.11'
PROJECT_DESCRIPTION = "Android DroneKit client library."
PROJECT_LABELS = ['3DR', '3DR Services', 'DroneAPI', 'Android', 'DroneKit']
PROJECT_LICENSES = ['Apache-2.0']
@@ -17,7 +17,7 @@ android {
defaultConfig {
minSdkVersion 14
targetSdkVersion 21
versionCode 20309
versionCode 20311
versionName PUBLISH_VERSION
}

Original file line number Diff line number Diff line change
@@ -31,18 +31,62 @@ public DroneSharePrefs getDroneSharePrefs() {
return droneSharePrefs;
}

public String getUniqueId(){
final String uniqueId;
switch(connectionType){

case ConnectionType.TYPE_UDP:
int udpPort = ConnectionType.DEFAULT_UDP_SERVER_PORT;
if(paramsBundle != null){
udpPort = paramsBundle.getInt(ConnectionType.EXTRA_UDP_SERVER_PORT, udpPort);
}
uniqueId = "udp." + udpPort;
break;

case ConnectionType.TYPE_BLUETOOTH:
String btAddress = null;
if(paramsBundle != null){
btAddress = paramsBundle.getString(ConnectionType.EXTRA_BLUETOOTH_ADDRESS);
}

uniqueId = btAddress == null ? "bluetooth" : "bluetooth." + btAddress;
break;

case ConnectionType.TYPE_TCP:
String tcpIp = null;
int tcpPort = ConnectionType.DEFAULT_TCP_SERVER_PORT;
if(paramsBundle != null){
tcpIp = paramsBundle.getString(ConnectionType.EXTRA_TCP_SERVER_IP);
tcpPort = paramsBundle.getInt(ConnectionType.EXTRA_TCP_SERVER_PORT, tcpPort);
}

uniqueId = "tcp" + "." + tcpPort + (tcpIp == null ? "" : "." + tcpIp);
break;

case ConnectionType.TYPE_USB:
uniqueId = "usb";
break;

default:
uniqueId = "";
break;
}

return uniqueId;
}

@Override
public boolean equals(Object o){
if(this == o) return true;
if(!(o instanceof ConnectionParameter)) return false;

ConnectionParameter that = (ConnectionParameter) o;
return toString().equals(that.toString());
return getUniqueId().equals(that.getUniqueId());
}

@Override
public int hashCode(){
return toString().hashCode();
return getUniqueId().hashCode();
}

@Override
Original file line number Diff line number Diff line change
@@ -29,7 +29,29 @@ public class ConnectionType {
/**
* Default value for the upd server port.
*/
public static final int DEFAULT_UPD_SERVER_PORT = 14550;
public static final int DEFAULT_UDP_SERVER_PORT = 14550;

/**
* Key used to retrieve the ip address of the udp server to ping.
*/
public static final String EXTRA_UDP_PING_RECEIVER_IP = "extra_udp_ping_receiver_ip";

/**
* Key used to retrieve the port of the udp server to ping.
*/
public static final String EXTRA_UDP_PING_RECEIVER_PORT = "extra_udp_ping_receiver_port";

/**
* Ping payload.
*/
public static final String EXTRA_UDP_PING_PAYLOAD = "extra_udp_ping_payload";

/**
* How often should the udp ping be performed.
*/
public static final String EXTRA_UDP_PING_PERIOD = "extra_udp_ping_period";

public static final long DEFAULT_UDP_PING_PERIOD = 10000l; //10 seconds

/**
* TCP connection type
Original file line number Diff line number Diff line change
@@ -8,7 +8,7 @@
*/
public class VersionUtils {

public static final int LIB_VERSION = 20200;
public static final int LIB_VERSION = 20202;

public static int getVersion(Context context){
try {
Original file line number Diff line number Diff line change
@@ -39,6 +39,8 @@
import org.droidplanner.services.android.utils.analytics.GAUtils;
import org.droidplanner.services.android.utils.file.IO.CameraInfoLoader;

import java.net.InetAddress;
import java.net.UnknownHostException;
import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.ConcurrentHashMap;
@@ -65,7 +67,7 @@ public class DroidPlannerService extends Service {
/**
* Caches mavlink connections per connection type.
*/
final ConcurrentHashMap<ConnectionParameter, AndroidMavLinkConnection> mavConnections = new ConcurrentHashMap<>();
final ConcurrentHashMap<String, AndroidMavLinkConnection> mavConnections = new ConcurrentHashMap<>();

/**
* Caches drone managers per connection type.
@@ -137,12 +139,12 @@ void disconnectDroneManager(DroneManager droneMgr, String appId) throws Connecti

void connectMAVConnection(ConnectionParameter connParams, String listenerTag,
MavLinkConnectionListener listener) {
AndroidMavLinkConnection conn = mavConnections.get(connParams);
AndroidMavLinkConnection conn = mavConnections.get(connParams.getUniqueId());
final int connectionType = connParams.getConnectionType();
final Bundle paramsBundle = connParams.getParamsBundle();
if (conn == null) {

//Create a new mavlink connection
final int connectionType = connParams.getConnectionType();
final Bundle paramsBundle = connParams.getParamsBundle();

switch (connectionType) {
case ConnectionType.TYPE_USB:
@@ -169,8 +171,8 @@ void connectMAVConnection(ConnectionParameter connParams, String listenerTag,
break;

case ConnectionType.TYPE_UDP:
final int udpServerPort = paramsBundle.getInt(ConnectionType
.EXTRA_UDP_SERVER_PORT, ConnectionType.DEFAULT_UPD_SERVER_PORT);
final int udpServerPort = paramsBundle
.getInt(ConnectionType.EXTRA_UDP_SERVER_PORT, ConnectionType.DEFAULT_UDP_SERVER_PORT);
conn = new AndroidUdpConnection(getApplicationContext(), udpServerPort);
Log.d(TAG, "Connecting over udp.");
break;
@@ -180,7 +182,26 @@ void connectMAVConnection(ConnectionParameter connParams, String listenerTag,
return;
}

mavConnections.put(connParams, conn);
mavConnections.put(connParams.getUniqueId(), conn);
}

if (connectionType == ConnectionType.TYPE_UDP) {
final String pingIpAddress = paramsBundle.getString(ConnectionType.EXTRA_UDP_PING_RECEIVER_IP);
if (!TextUtils.isEmpty(pingIpAddress)) {
try {
final InetAddress resolvedAddress = InetAddress.getByName(pingIpAddress);

final int pingPort = paramsBundle.getInt(ConnectionType.EXTRA_UDP_PING_RECEIVER_PORT);
final long pingPeriod = paramsBundle.getLong(ConnectionType.EXTRA_UDP_PING_PERIOD,
ConnectionType.DEFAULT_UDP_PING_PERIOD);
final byte[] pingPayload = paramsBundle.getByteArray(ConnectionType.EXTRA_UDP_PING_PAYLOAD);

((AndroidUdpConnection) conn).addPingTarget(resolvedAddress, pingPort, pingPeriod, pingPayload);

} catch (UnknownHostException e) {
Log.e(TAG, "Unable to resolve UDP ping server ip address.", e);
}
}
}

conn.addMavLinkConnectionListener(listenerTag, listener);
@@ -196,23 +217,23 @@ void connectMAVConnection(ConnectionParameter connParams, String listenerTag,
}

void addLoggingFile(ConnectionParameter connParams, String tag, String loggingFilePath) {
AndroidMavLinkConnection conn = mavConnections.get(connParams);
AndroidMavLinkConnection conn = mavConnections.get(connParams.getUniqueId());
if (conn == null)
return;

conn.addLoggingPath(tag, loggingFilePath);
}

void removeLoggingFile(ConnectionParameter connParams, String tag) {
AndroidMavLinkConnection conn = mavConnections.get(connParams);
AndroidMavLinkConnection conn = mavConnections.get(connParams.getUniqueId());
if (conn == null)
return;

conn.removeLoggingPath(tag);
}

void disconnectMAVConnection(ConnectionParameter connParams, String listenerTag) {
final AndroidMavLinkConnection conn = mavConnections.get(connParams);
final AndroidMavLinkConnection conn = mavConnections.get(connParams.getUniqueId());
if (conn == null)
return;

@@ -289,6 +310,7 @@ public void onCreate() {
updateForegroundNotification();
}

@SuppressLint("NewApi")
private void updateForegroundNotification() {
final Context context = getApplicationContext();

Original file line number Diff line number Diff line change
@@ -685,6 +685,7 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) {
break;

case CONNECTION_FAILED:
disconnect();
onConnectionFailed("");
break;

Original file line number Diff line number Diff line change
@@ -30,7 +30,7 @@ private DroidPlannerService getService() {
}

public boolean sendData(ConnectionParameter connParams, MAVLinkPacket packet) {
final AndroidMavLinkConnection mavConnection = getService().mavConnections.get(connParams);
final AndroidMavLinkConnection mavConnection = getService().mavConnections.get(connParams.getUniqueId());
if (mavConnection == null) return false;

if (mavConnection.getConnectionStatus() != MavLinkConnection.MAVLINK_DISCONNECTED) {
@@ -42,16 +42,15 @@ public boolean sendData(ConnectionParameter connParams, MAVLinkPacket packet) {
}

public int getConnectionStatus(ConnectionParameter connParams, String tag) {
final AndroidMavLinkConnection mavConnection = getService().mavConnections.get(connParams);
final AndroidMavLinkConnection mavConnection = getService().mavConnections.get(connParams.getUniqueId());
if (mavConnection == null || !mavConnection.hasMavLinkConnectionListener(tag)) {
return MavLinkConnection.MAVLINK_DISCONNECTED;
}

return mavConnection.getConnectionStatus();
}

public void connectMavLink(ConnectionParameter connParams, String tag,
MavLinkConnectionListener listener) {
public void connectMavLink(ConnectionParameter connParams, String tag, MavLinkConnectionListener listener) {
getService().connectMAVConnection(connParams, tag, listener);
}

Loading

0 comments on commit b85e70d

Please sign in to comment.