From ea124d8210ef5ef3ba2a8acea6e7d1b613b9f5ee Mon Sep 17 00:00:00 2001 From: Nikhil Khare Date: Thu, 21 Jan 2016 15:16:00 -0800 Subject: [PATCH 01/37] DRON-19 (Video pipeline improvement) --- ClientLib/build.gradle | 6 +- .../java/com/o3dr/android/client/Drone.java | 3 +- .../o3dr/android/client/DroneObserver.java | 1 - .../android/client/IVideoStreamCallback.java | 8 + .../android/client/VideoStreamObserver.java | 89 +++++++++++ .../o3dr/android/client/apis/CameraApi.java | 47 +++++- .../android/client/apis/ExperimentalApi.java | 90 ++++++++++- .../client/apis/solo/SoloCameraApi.java | 37 +++-- .../lib/drone/action/CameraActions.java | 4 + .../lib/model/AbstractCommandListener.java | 3 - .../services/android/lib/model/IDroneApi.aidl | 1 - .../lib/model/VideoStreamListener.java | 11 ++ ServiceApp/build.gradle | 6 +- .../services/android/api/DroneApi.java | 39 ++++- .../drone/autopilot/apm/solo/ArduSolo.java | 4 +- .../drone/autopilot/apm/solo/SoloComp.java | 1 - .../generic/GenericMavLinkDrone.java | 17 ++- .../android/utils/CommonApiUtils.java | 32 +++- .../android/utils/video/VideoManager.java | 142 +++++++++++++++--- build.gradle | 2 +- samples/StarterApp/build.gradle | 6 +- .../o3dr/sample/hellodrone/MainActivity.java | 100 +++++++++++- 22 files changed, 564 insertions(+), 85 deletions(-) create mode 100644 ClientLib/src/main/java/com/o3dr/android/client/IVideoStreamCallback.java create mode 100644 ClientLib/src/main/java/com/o3dr/android/client/VideoStreamObserver.java create mode 100644 ClientLib/src/main/java/com/o3dr/services/android/lib/model/VideoStreamListener.java diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index f1db4ec1fa..7a5deef246 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -40,9 +40,9 @@ android { abortOnError false } - dexOptions { - dexInProcess = true - } + //dexOptions { + // dexInProcess = true + //} testOptions { unitTests.returnDefaultValues = true 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 0d974d2616..84d54528d5 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/Drone.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/Drone.java @@ -57,7 +57,6 @@ * Created by fhuya on 11/4/14. */ public class Drone { - private static final String CLAZZ_NAME = Drone.class.getName(); private static final String TAG = Drone.class.getSimpleName(); @@ -132,7 +131,7 @@ void init(ControlTower controlTower, Handler handler) { this.droneObserver = new DroneObserver(this); } - Context getContext(){ + Context getContext() { return this.context; } diff --git a/ClientLib/src/main/java/com/o3dr/android/client/DroneObserver.java b/ClientLib/src/main/java/com/o3dr/android/client/DroneObserver.java index ab301dc2fa..3e85011a68 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/DroneObserver.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/DroneObserver.java @@ -3,7 +3,6 @@ import android.os.Bundle; import android.os.RemoteException; -import com.o3dr.services.android.lib.drone.connection.ConnectionResult; import com.o3dr.services.android.lib.model.IObserver; /** diff --git a/ClientLib/src/main/java/com/o3dr/android/client/IVideoStreamCallback.java b/ClientLib/src/main/java/com/o3dr/android/client/IVideoStreamCallback.java new file mode 100644 index 0000000000..330ddafbb7 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/android/client/IVideoStreamCallback.java @@ -0,0 +1,8 @@ +package com.o3dr.android.client; + +/** + * TODO + */ +public interface IVideoStreamCallback { + void getVideoStreamPackets(byte[] packets); +} diff --git a/ClientLib/src/main/java/com/o3dr/android/client/VideoStreamObserver.java b/ClientLib/src/main/java/com/o3dr/android/client/VideoStreamObserver.java new file mode 100644 index 0000000000..f511c937c3 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/android/client/VideoStreamObserver.java @@ -0,0 +1,89 @@ +package com.o3dr.android.client; + +import android.content.Context; +import android.os.Handler; +import android.util.Log; + +import com.o3dr.android.client.utils.connection.IpConnectionListener; +import com.o3dr.android.client.utils.connection.UdpConnection; + +import java.nio.ByteBuffer; + +/** + * TODO + */ +public class VideoStreamObserver implements IpConnectionListener { + private static final String TAG = VideoStreamObserver.class.getSimpleName(); + + private static final int UDP_BUFFER_SIZE = 1500; + private static final long RECONNECT_COUNTDOWN_IN_MILLIS = 1000l; + + private Context context; + private UdpConnection linkConn; + private Handler handler; + + private int linkPort = -1; + + private IVideoStreamCallback callback; + + public VideoStreamObserver(Context context, Handler handler, IVideoStreamCallback callback) { + this.context = context; + this.handler = handler; + this.callback = callback; + } + + private final Runnable reconnectTask = new Runnable() { + @Override + public void run() { + handler.removeCallbacks(reconnectTask); + if(linkConn != null) + linkConn.connect(); + } + }; + + private void start(int udpPort) { + if (this.linkConn == null || udpPort != this.linkPort){ + this.linkConn = new UdpConnection(handler, udpPort, UDP_BUFFER_SIZE, true, 42); + this.linkConn.setIpConnectionListener(this); + this.linkPort = udpPort; + } + + handler.removeCallbacks(reconnectTask); + + Log.d(TAG, "Connecting to video stream..."); + this.linkConn.connect(); + } + + private void stop() { + Log.d(TAG, "Stopping video manager"); + + handler.removeCallbacks(reconnectTask); + + if (this.linkConn != null) { + // Break the link + this.linkConn.disconnect(); + this.linkConn = null; + } + + this.linkPort = -1; + } + + @Override + public void onIpConnected() { + Log.d(TAG, "Connected to video stream"); + + handler.removeCallbacks(reconnectTask); + } + + @Override + public void onIpDisconnected() { + Log.d(TAG, "Video stream disconnected"); + + handler.postDelayed(reconnectTask, RECONNECT_COUNTDOWN_IN_MILLIS); + } + + @Override + public void onPacketReceived(ByteBuffer packetBuffer) { + callback.getVideoStreamPackets(packetBuffer.array()); + } +} diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/CameraApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/CameraApi.java index e143a79573..dcd3aa72dd 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/CameraApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/CameraApi.java @@ -7,12 +7,14 @@ import com.o3dr.android.client.Drone; import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.model.AbstractCommandListener; +import com.o3dr.services.android.lib.model.VideoStreamListener; import com.o3dr.services.android.lib.model.action.Action; import java.util.concurrent.ConcurrentHashMap; import static com.o3dr.services.android.lib.drone.action.CameraActions.ACTION_START_VIDEO_STREAM; import static com.o3dr.services.android.lib.drone.action.CameraActions.ACTION_STOP_VIDEO_STREAM; +import static com.o3dr.services.android.lib.drone.action.CameraActions.ACTION_START_VIDEO_STREAM_FOR_OBSERVER; import static com.o3dr.services.android.lib.drone.action.CameraActions.EXTRA_VIDEO_DISPLAY; import static com.o3dr.services.android.lib.drone.action.CameraActions.EXTRA_VIDEO_ENABLE_LOCAL_RECORDING; import static com.o3dr.services.android.lib.drone.action.CameraActions.EXTRA_VIDEO_LOCAL_RECORDING_FILENAME; @@ -27,7 +29,6 @@ * @since 2.6.8 */ public class CameraApi extends Api { - private static final ConcurrentHashMap apiCache = new ConcurrentHashMap<>(); private static final Builder apiBuilder = new Builder() { @Override @@ -73,13 +74,14 @@ private CameraApi(Drone drone) { * Attempt to grab ownership and start the video stream from the connected drone. Can fail if * the video stream is already owned by another client. * - * @param surface Surface object onto which the video is decoded. - * @param tag Video tag. - * @param videoProps Non-null video properties. @see VIDEO_PROPS_UDP_PORT - * @param listener Register a callback to receive update of the command execution status. + * @param surface Surface object onto which the video is decoded. + * @param tag Video tag. + * @param videoProps Non-null video properties. @see VIDEO_PROPS_UDP_PORT + * @param listener Register a callback to receive update of the command execution status. * @since 2.6.8 */ - public void startVideoStream(@NonNull final Surface surface, final String tag, @NonNull Bundle videoProps, final AbstractCommandListener listener) { + public void startVideoStream(@NonNull final Surface surface, final String tag, + @NonNull Bundle videoProps, final AbstractCommandListener listener) { if (surface == null || videoProps == null) { postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); return; @@ -105,4 +107,37 @@ public void stopVideoStream(final String tag, final AbstractCommandListener list params.putString(EXTRA_VIDEO_TAG, tag); drone.performAsyncActionOnDroneThread(new Action(ACTION_STOP_VIDEO_STREAM, params), listener); } + + /** + * Attempt to grab ownership and start the video stream from the connected drone. Can fail if + * the video stream is already owned by another client. + * + * @param tag Video tag. + * @param videoProps Non-null video properties. @see VIDEO_PROPS_UDP_PORT + * @param listener Register a callback to receive update of the command execution status. + * @since 2.6.8 + */ + public void startVideoStream(final String tag, @NonNull Bundle videoProps, + final VideoStreamListener listener) { + final Bundle params = new Bundle(); + params.putString(EXTRA_VIDEO_TAG, tag); + params.putBundle(EXTRA_VIDEO_PROPERTIES, videoProps); + + drone.performAsyncActionOnDroneThread(new Action(ACTION_START_VIDEO_STREAM_FOR_OBSERVER, + params), listener); + } + + /** + * Stop the video stream from the connected drone, and release ownership. + * + * @param tag Video tag. + * @param listener Register a callback to receive update of the command execution status. + * @since 2.6.8 + */ + public void stopVideoStream(final String tag, final VideoStreamListener listener) { + final Bundle params = new Bundle(); + params.putString(EXTRA_VIDEO_TAG, tag); + + drone.performAsyncActionOnDroneThread(new Action(ACTION_STOP_VIDEO_STREAM, params), listener); + } } diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java index 2714b118ae..396461515c 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java @@ -3,11 +3,17 @@ import android.os.Bundle; import com.o3dr.android.client.Drone; +import com.o3dr.android.client.VideoStreamObserver; import com.o3dr.services.android.lib.coordinate.LatLongAlt; +import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.mavlink.MavlinkMessageWrapper; import com.o3dr.services.android.lib.model.AbstractCommandListener; +import com.o3dr.services.android.lib.model.VideoStreamListener; import com.o3dr.services.android.lib.model.action.Action; +import java.text.SimpleDateFormat; +import java.util.Date; +import java.util.Locale; import java.util.concurrent.ConcurrentHashMap; import static com.o3dr.services.android.lib.drone.action.ExperimentalActions.ACTION_SEND_MAVLINK_MESSAGE; @@ -22,12 +28,11 @@ import static com.o3dr.services.android.lib.drone.action.ExperimentalActions.EXTRA_SERVO_PWM; import static com.o3dr.services.android.lib.drone.action.ExperimentalActions.EXTRA_SET_ROI_LAT_LONG_ALT; - /** * Contains drone commands with no defined interaction model yet. */ public class ExperimentalApi extends Api { - + private static final SimpleDateFormat FILE_DATE_FORMAT = new SimpleDateFormat("yyyy_MM_dd_HH_mm_ss", Locale.US); private static final ConcurrentHashMap experimentalApiCache = new ConcurrentHashMap<>(); private static final Builder apiBuilder = new Builder() { @Override @@ -36,6 +41,11 @@ public ExperimentalApi build(Drone drone) { } }; + private static final int SOLO_STREAM_UDP_PORT = 5600; + + private final CapabilityApi capabilityChecker; + private final CameraApi cameraApi; + /** * Retrieves an ExperimentalApi instance. * @@ -50,6 +60,8 @@ public static ExperimentalApi getApi(final Drone drone) { private ExperimentalApi(Drone drone) { this.drone = drone; + this.capabilityChecker = CapabilityApi.getApi(drone); + this.cameraApi = CameraApi.getApi(drone); } /** @@ -149,4 +161,78 @@ public void setServo(int channel, int pwm, AbstractCommandListener listener) { params.putInt(EXTRA_SERVO_PWM, pwm); drone.performAsyncActionOnDroneThread(new Action(ACTION_SET_SERVO, params), listener); } + + /** + * Attempt to grab ownership and start the video stream from the connected drone. Can fail if + * the video stream is already owned by another client. + * + * @param tag Video tag. + * @param enableLocalRecording Set to true to enable local recording, false to disable it. + * @param listener Register a callback to receive update of the command execution status. + * + * @since 2.5.0 + */ + public void startVideoStream(final String tag, final boolean enableLocalRecording, + final VideoStreamListener listener) { + capabilityChecker.checkFeatureSupport(CapabilityApi.FeatureIds.SOLO_VIDEO_STREAMING, + new CapabilityApi.FeatureSupportListener() { + @Override + public void onFeatureSupportResult(String featureId, int result, Bundle resultInfo) { + switch (result) { + case CapabilityApi.FEATURE_SUPPORTED: + final Bundle videoProps = new Bundle(); + videoProps.putInt(CameraApi.VIDEO_PROPS_UDP_PORT, SOLO_STREAM_UDP_PORT); + + videoProps.putBoolean(CameraApi.VIDEO_ENABLE_LOCAL_RECORDING, enableLocalRecording); + if (enableLocalRecording) { + String localRecordingFilename = "solo_stream_" + FILE_DATE_FORMAT.format(new Date()); + videoProps.putString(CameraApi.VIDEO_LOCAL_RECORDING_FILENAME, localRecordingFilename); + } + + cameraApi.startVideoStream(tag, videoProps, listener); + break; + + case CapabilityApi.FEATURE_UNSUPPORTED: + postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); + break; + + default: + postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); + break; + } + } + }); + + } + + /** + * Stop the video stream from the connected drone, and release ownership. + * + * @param tag Video tag. + * @param listener Register a callback to receive update of the command execution status. + * + * @since 2.5.0 + */ + public void stopVideoStream(final String tag, final VideoStreamListener listener) { + capabilityChecker.checkFeatureSupport(CapabilityApi.FeatureIds.SOLO_VIDEO_STREAMING, + new CapabilityApi.FeatureSupportListener() { + @Override + public void onFeatureSupportResult(String featureId, int result, Bundle resultInfo) { + switch (result) { + + case CapabilityApi.FEATURE_SUPPORTED: + cameraApi.stopVideoStream(tag, listener); + break; + + case CapabilityApi.FEATURE_UNSUPPORTED: + postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); + break; + + default: + postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); + break; + } + } + }); + } } diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/solo/SoloCameraApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/solo/SoloCameraApi.java index caadb19ee2..378bdaf2e9 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/solo/SoloCameraApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/solo/SoloCameraApi.java @@ -7,7 +7,6 @@ import com.o3dr.android.client.Drone; import com.o3dr.android.client.apis.CameraApi; import com.o3dr.android.client.apis.CapabilityApi; -import com.o3dr.services.android.lib.drone.action.CameraActions; import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloGoproConstants; import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloGoproRecord; @@ -27,7 +26,6 @@ * @since 2.5.0 */ public class SoloCameraApi extends SoloApi { - private static final SimpleDateFormat FILE_DATE_FORMAT = new SimpleDateFormat("yyyy_MM_dd_HH_mm_ss", Locale.US); private static final ConcurrentHashMap soloCameraApiCache = new ConcurrentHashMap<>(); @@ -121,8 +119,9 @@ public void stopVideoRecording(final AbstractCommandListener listener) { sendVideoRecordingCommand(SoloGoproConstants.STOP_RECORDING, listener); } - private void sendVideoRecordingCommand(@SoloGoproConstants.RecordCommand final int recordCommand, final AbstractCommandListener listener) { - //Set the gopro to video mode + private void sendVideoRecordingCommand(@SoloGoproConstants.RecordCommand final int recordCommand, + final AbstractCommandListener listener) { + // Set the gopro to video mode. switchCameraCaptureMode(SoloGoproConstants.CAPTURE_MODE_VIDEO, new AbstractCommandListener() { @Override public void onSuccess() { @@ -147,7 +146,8 @@ public void onTimeout() { }); } - public void startVideoStream(final Surface surface, final String tag, final AbstractCommandListener listener) { + public void startVideoStream(final Surface surface, final String tag, + final AbstractCommandListener listener) { startVideoStream(surface, tag, false, listener); } @@ -155,14 +155,15 @@ public void startVideoStream(final Surface surface, final String tag, final Abst * Attempt to grab ownership and start the video stream from the connected drone. Can fail if * the video stream is already owned by another client. * - * @param surface Surface object onto which the video is decoded. - * @param tag Video tag. + * @param surface Surface object onto which the video is decoded. + * @param tag Video tag. * @param enableLocalRecording Set to true to enable local recording, false to disable it. - * @param listener Register a callback to receive update of the command execution status. + * @param listener Register a callback to receive update of the command execution status. * * @since 2.5.0 */ - public void startVideoStream(final Surface surface, final String tag, final boolean enableLocalRecording, final AbstractCommandListener listener) { + public void startVideoStream(final Surface surface, final String tag, final boolean enableLocalRecording, + final AbstractCommandListener listener) { if (surface == null) { postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); return; @@ -265,14 +266,18 @@ public void onFeatureSupportResult(String featureId, int result, Bundle resultIn * @param listener Register a callback to receive update of the command execution status. * @since 2.6.8 */ - public void switchCameraCaptureMode(@SoloGoproConstants.CaptureMode byte captureMode, final AbstractCommandListener listener) { - final SoloGoproSetRequest captureModeRequest = new SoloGoproSetRequest((short) GOPRO_COMMAND.GOPRO_COMMAND_CAPTURE_MODE, captureMode); + public void switchCameraCaptureMode(@SoloGoproConstants.CaptureMode byte captureMode, + final AbstractCommandListener listener) { + final SoloGoproSetRequest captureModeRequest = + new SoloGoproSetRequest((short) GOPRO_COMMAND.GOPRO_COMMAND_CAPTURE_MODE, captureMode); sendMessage(captureModeRequest, listener); } - private void sendExtendedRequest(AbstractCommandListener listener, int command, byte value1, byte value2, byte value3, byte value4){ + private void sendExtendedRequest(AbstractCommandListener listener, int command, byte value1, + byte value2, byte value3, byte value4){ byte[] values = {value1, value2, value3, value4}; - SoloGoproSetExtendedRequest extendedRequest = new SoloGoproSetExtendedRequest((short) command, values); + SoloGoproSetExtendedRequest extendedRequest = + new SoloGoproSetExtendedRequest((short) command, values); sendMessage(extendedRequest, listener); } @@ -288,8 +293,10 @@ private void sendExtendedRequest(AbstractCommandListener listener, int command, * @param fieldOfView * @param flags */ - public void updateVideoSettings(byte resolution, byte frameRate, byte fieldOfView, byte flags, AbstractCommandListener listener){ - sendExtendedRequest(listener, GOPRO_COMMAND.GOPRO_COMMAND_VIDEO_SETTINGS, resolution, frameRate, fieldOfView, flags); + public void updateVideoSettings(byte resolution, byte frameRate, byte fieldOfView, byte flags, + AbstractCommandListener listener){ + sendExtendedRequest(listener, GOPRO_COMMAND.GOPRO_COMMAND_VIDEO_SETTINGS, resolution, + frameRate, fieldOfView, flags); } public void setCameraPhotoResolution(byte photoResolution, AbstractCommandListener listener){ diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/CameraActions.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/CameraActions.java index 342dfd3662..5615578513 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/CameraActions.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/CameraActions.java @@ -15,10 +15,14 @@ private CameraActions() { public static final String EXTRA_VIDEO_DISPLAY = "extra_video_display"; public static final String EXTRA_VIDEO_TAG = "extra_video_tag"; + public static final String ACTION_START_VIDEO_STREAM_FOR_OBSERVER = PACKAGE_NAME + ".START_VIDEO_STREAM_FOR_OBSERVER"; + public static final String EXTRA_VIDEO_PROPERTIES = "extra_video_properties"; public static final String EXTRA_VIDEO_PROPS_UDP_PORT = "extra_video_props_udp_port"; public static final String EXTRA_VIDEO_ENABLE_LOCAL_RECORDING = "extra_video_enable_local_recording"; public static final String EXTRA_VIDEO_LOCAL_RECORDING_FILENAME = "extra_video_local_recording_filename"; public static final String ACTION_STOP_VIDEO_STREAM = PACKAGE_NAME + ".STOP_VIDEO_STREAM"; + + public static final String ACTION_STOP_VIDEO_STREAM_FOR_OBSERVER = PACKAGE_NAME + ".STOP_VIDEO_STREAM_FOR_OBSERVER"; } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/model/AbstractCommandListener.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/model/AbstractCommandListener.java index effda25894..d40d7e3555 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/model/AbstractCommandListener.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/model/AbstractCommandListener.java @@ -1,12 +1,9 @@ package com.o3dr.services.android.lib.model; -import android.os.RemoteException; - /** * Created by Fredia Huya-Kouadio on 7/5/15. */ public abstract class AbstractCommandListener extends ICommandListener.Stub { - @Override public abstract void onSuccess(); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/model/IDroneApi.aidl b/ClientLib/src/main/java/com/o3dr/services/android/lib/model/IDroneApi.aidl index 5eb66b89a6..30172c0304 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/model/IDroneApi.aidl +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/model/IDroneApi.aidl @@ -71,5 +71,4 @@ interface IDroneApi { * @param listener Register a callback to be invoken when the action is executed. */ oneway void executeAsyncAction(in Action action, ICommandListener listener); - } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/model/VideoStreamListener.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/model/VideoStreamListener.java new file mode 100644 index 0000000000..02749e8c91 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/model/VideoStreamListener.java @@ -0,0 +1,11 @@ +package com.o3dr.services.android.lib.model; + +/** + * Listens to changes in the video state. + */ +public abstract class VideoStreamListener extends AbstractCommandListener{ + + public abstract void onVideoStarted(); + + public abstract void onVideoStopped(); +} diff --git a/ServiceApp/build.gradle b/ServiceApp/build.gradle index 3d3d696783..a0a5b7a86e 100644 --- a/ServiceApp/build.gradle +++ b/ServiceApp/build.gradle @@ -214,9 +214,9 @@ android { unitTests.returnDefaultValues = true } - dexOptions { - dexInProcess = true - } + //dexOptions { + // dexInProcess = true + //} } def getAppKeystoreFile() { diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index 4cd38ef7bc..137070e49c 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -12,6 +12,7 @@ import com.MAVLink.Messages.MAVLinkMessage; import com.MAVLink.ardupilotmega.msg_mag_cal_progress; import com.MAVLink.ardupilotmega.msg_mag_cal_report; +import com.o3dr.android.client.VideoStreamObserver; import com.o3dr.services.android.lib.drone.action.CameraActions; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.action.ConnectionActions; @@ -31,11 +32,13 @@ import com.o3dr.services.android.lib.drone.property.State; import com.o3dr.services.android.lib.gcs.event.GCSEvent; import com.o3dr.services.android.lib.mavlink.MavlinkMessageWrapper; +import com.o3dr.services.android.lib.model.AbstractCommandListener; import com.o3dr.services.android.lib.model.IApiListener; import com.o3dr.services.android.lib.model.ICommandListener; import com.o3dr.services.android.lib.model.IDroneApi; import com.o3dr.services.android.lib.model.IMavlinkObserver; import com.o3dr.services.android.lib.model.IObserver; +import com.o3dr.services.android.lib.model.VideoStreamListener; import com.o3dr.services.android.lib.model.action.Action; import org.droidplanner.services.android.communication.connection.SoloConnection; @@ -52,6 +55,7 @@ import java.util.ArrayList; import java.util.List; import java.util.NoSuchElementException; +import java.util.concurrent.ConcurrentHashMap; import java.util.concurrent.ConcurrentLinkedQueue; import timber.log.Timber; @@ -184,11 +188,11 @@ public boolean isConnected() { } private ConnectionParameter checkConnectionParameter(ConnectionParameter connParams) throws ConnectionException { - if(connParams == null){ + if (connParams == null){ throw new ConnectionException("Invalid connection parameters"); } - if(SoloConnection.isSoloConnection(context, connParams)){ + if (SoloConnection.isSoloConnection(context, connParams)){ ConnectionParameter update = SoloConnection.getSoloConnectionParameterIfPossible(context); if(update != null){ return update; @@ -272,7 +276,7 @@ public void executeAction(Action action, ICommandListener listener) throws Remot Drone drone = getDrone(); switch (type) { - //CONNECTION ACTIONS + // CONNECTION ACTIONS case ConnectionActions.ACTION_CONNECT: ConnectionParameter parameter = data.getParcelable(ConnectionActions.EXTRA_CONNECT_PARAMETER); connect(parameter); @@ -282,7 +286,7 @@ public void executeAction(Action action, ICommandListener listener) throws Remot disconnect(); break; - //CAMERA ACTIONS + // CAMERA ACTIONS case CameraActions.ACTION_START_VIDEO_STREAM: { Surface videoSurface = data.getParcelable(CameraActions.EXTRA_VIDEO_DISPLAY); String videoTag = data.getString(CameraActions.EXTRA_VIDEO_TAG, ""); @@ -296,13 +300,36 @@ public void executeAction(Action action, ICommandListener listener) throws Remot videoProps.putInt(CameraActions.EXTRA_VIDEO_PROPS_UDP_PORT, VideoManager.ARTOO_UDP_PORT); } - CommonApiUtils.startVideoStream(drone, videoProps, ownerId, videoTag, videoSurface, listener); + CommonApiUtils.startVideoStream(drone, videoProps, ownerId, videoTag, videoSurface, + (AbstractCommandListener) listener); + break; + } + + case CameraActions.ACTION_START_VIDEO_STREAM_FOR_OBSERVER: { + String videoTag = data.getString(CameraActions.EXTRA_VIDEO_TAG, ""); + + Bundle videoProps = data.getBundle(CameraActions.EXTRA_VIDEO_PROPERTIES); + if (videoProps == null) { + //Only case where it's null is when interacting with a deprecated client version. + //In this case, we assume that the client is attempting to start a solo stream, since that's + //the only api that was exposed. + videoProps = new Bundle(); + videoProps.putInt(CameraActions.EXTRA_VIDEO_PROPS_UDP_PORT, VideoManager.ARTOO_UDP_PORT); + } + + CommonApiUtils.startVideoStream(drone, videoProps, ownerId, videoTag, (VideoStreamListener) listener); break; } case CameraActions.ACTION_STOP_VIDEO_STREAM: { String videoTag = data.getString(CameraActions.EXTRA_VIDEO_TAG, ""); - CommonApiUtils.stopVideoStream(drone, ownerId, videoTag, listener); + CommonApiUtils.stopVideoStream(drone, ownerId, videoTag, (AbstractCommandListener) listener); + break; + } + + case CameraActions.ACTION_STOP_VIDEO_STREAM_FOR_OBSERVER: { + String videoTag = data.getString(CameraActions.EXTRA_VIDEO_TAG, ""); + CommonApiUtils.stopVideoStream(drone, ownerId, videoTag, (VideoStreamListener) listener); break; } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/solo/ArduSolo.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/solo/ArduSolo.java index ad57005307..a497d232df 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/solo/ArduSolo.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/solo/ArduSolo.java @@ -28,6 +28,7 @@ import com.o3dr.services.android.lib.drone.companion.solo.tlv.TLVPacket; import com.o3dr.services.android.lib.drone.property.DroneAttribute; import com.o3dr.services.android.lib.drone.property.State; +import com.o3dr.services.android.lib.model.AbstractCommandListener; import com.o3dr.services.android.lib.model.ICommandListener; import com.o3dr.services.android.lib.model.action.Action; @@ -219,7 +220,8 @@ protected void resetVideoManager() { } @Override - public void startVideoStream(Bundle videoProps, String appId, String newVideoTag, Surface videoSurface, final ICommandListener listener) { + public void startVideoStream(Bundle videoProps, String appId, String newVideoTag, Surface videoSurface, + final AbstractCommandListener listener) { if (!soloComp.hasStreamingPermission()) { postErrorEvent(CommandExecutionError.COMMAND_DENIED, listener); return; diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/solo/SoloComp.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/solo/SoloComp.java index 105f078611..8a216b06bd 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/solo/SoloComp.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/apm/solo/SoloComp.java @@ -23,7 +23,6 @@ import com.o3dr.services.android.lib.drone.companion.solo.tlv.SoloMessageLocation; import com.o3dr.services.android.lib.drone.companion.solo.tlv.TLVMessageTypes; import com.o3dr.services.android.lib.drone.companion.solo.tlv.TLVPacket; -import com.o3dr.services.android.lib.model.AbstractCommandListener; import com.o3dr.services.android.lib.model.ICommandListener; import org.droidplanner.services.android.BuildConfig; 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 0fa4ace965..f4f422ddc3 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 @@ -21,6 +21,7 @@ import com.MAVLink.common.msg_vibration; import com.MAVLink.enums.MAV_MODE_FLAG; import com.MAVLink.enums.MAV_STATE; +import com.o3dr.android.client.VideoStreamObserver; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.action.CapabilityActions; @@ -45,7 +46,9 @@ import com.o3dr.services.android.lib.drone.property.VehicleMode; import com.o3dr.services.android.lib.drone.property.Vibration; import com.o3dr.services.android.lib.mavlink.MavlinkMessageWrapper; +import com.o3dr.services.android.lib.model.AbstractCommandListener; import com.o3dr.services.android.lib.model.ICommandListener; +import com.o3dr.services.android.lib.model.VideoStreamListener; import com.o3dr.services.android.lib.model.action.Action; import com.o3dr.services.android.lib.util.MathUtils; @@ -266,11 +269,21 @@ public void removeDroneListener(DroneInterfaces.OnDroneListener listener) { events.removeDroneListener(listener); } - public void startVideoStream(Bundle videoProps, String appId, String newVideoTag, Surface videoSurface, ICommandListener listener) { + public void startVideoStream(Bundle videoProps, String appId, String newVideoTag, Surface videoSurface, + AbstractCommandListener listener) { videoMgr.startVideoStream(videoProps, appId, newVideoTag, videoSurface, listener); } - public void stopVideoStream(String appId, String currentVideoTag, ICommandListener listener) { + public void stopVideoStream(String appId, String currentVideoTag, AbstractCommandListener listener) { + videoMgr.stopVideoStream(appId, currentVideoTag, listener); + } + + public void startVideoStream(Bundle videoProps, String appId, String newVideoTag, + VideoStreamListener listener) { + videoMgr.startVideoStream(videoProps, appId, newVideoTag, listener); + } + + public void stopVideoStream(String appId, String currentVideoTag, VideoStreamListener listener) { videoMgr.stopVideoStream(appId, currentVideoTag, listener); } diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/CommonApiUtils.java b/ServiceApp/src/org/droidplanner/services/android/utils/CommonApiUtils.java index cdfcb5ebeb..1268c14c92 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/CommonApiUtils.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/CommonApiUtils.java @@ -11,6 +11,7 @@ import com.MAVLink.ardupilotmega.msg_mag_cal_report; import com.MAVLink.enums.MAG_CAL_STATUS; import com.MAVLink.enums.MAV_TYPE; +import com.o3dr.android.client.VideoStreamObserver; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.attribute.AttributeType; @@ -40,6 +41,7 @@ import com.o3dr.services.android.lib.mavlink.MavlinkMessageWrapper; import com.o3dr.services.android.lib.model.AbstractCommandListener; import com.o3dr.services.android.lib.model.ICommandListener; +import com.o3dr.services.android.lib.model.VideoStreamListener; import org.droidplanner.services.android.core.MAVLink.MavLinkCommands; import org.droidplanner.services.android.core.MAVLink.command.doCmd.MavLinkDoCmds; @@ -932,9 +934,8 @@ public static MagnetometerCalibrationResult getMagnetometerCalibrationResult(msg msgReport.offdiag_x, msgReport.offdiag_y, msgReport.offdiag_z); } - public static void startVideoStream(Drone drone, Bundle videoProps, String appId, String videoTag, Surface videoSurface, - ICommandListener listener) { - + public static void startVideoStream(Drone drone, Bundle videoProps, String appId, String videoTag, + Surface videoSurface, AbstractCommandListener listener) { if (!(drone instanceof GenericMavLinkDrone)) { postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); return; @@ -944,7 +945,30 @@ public static void startVideoStream(Drone drone, Bundle videoProps, String appId mavLinkDrone.startVideoStream(videoProps, appId, videoTag, videoSurface, listener); } - public static void stopVideoStream(Drone drone, String appId, String videoTag, ICommandListener listener) { + public static void stopVideoStream(Drone drone, String appId, String videoTag, + AbstractCommandListener listener) { + if (!(drone instanceof GenericMavLinkDrone)) { + postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); + return; + } + + GenericMavLinkDrone mavLinkDrone = (GenericMavLinkDrone) drone; + mavLinkDrone.stopVideoStream(appId, videoTag, listener); + } + + public static void startVideoStream(Drone drone, Bundle videoProps, String appId, String videoTag, + VideoStreamListener listener) { + if (!(drone instanceof GenericMavLinkDrone)) { + postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); + return; + } + + GenericMavLinkDrone mavLinkDrone = (GenericMavLinkDrone) drone; + mavLinkDrone.startVideoStream(videoProps, appId, videoTag, listener); + } + + public static void stopVideoStream(Drone drone, String appId, String videoTag, + VideoStreamListener listener) { if (!(drone instanceof GenericMavLinkDrone)) { postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); return; diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java index f229846101..b1f1ad93a3 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java @@ -13,7 +13,9 @@ import com.o3dr.android.client.utils.connection.UdpConnection; import com.o3dr.services.android.lib.drone.action.CameraActions; import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; +import com.o3dr.services.android.lib.model.AbstractCommandListener; import com.o3dr.services.android.lib.model.ICommandListener; +import com.o3dr.services.android.lib.model.VideoStreamListener; import java.io.IOException; import java.nio.ByteBuffer; @@ -41,6 +43,8 @@ public class VideoManager implements IpConnectionListener { public static final int ARTOO_UDP_PORT = 5600; private static final int UDP_BUFFER_SIZE = 1500; + private boolean videoStreamObserverUsed = false; + public interface LinkListener { void onLinkConnected(); @@ -97,7 +101,7 @@ public void startDecoding(final int udpPort, final Surface surface, final Decode return; } - //Stop any in progress decoding. + // Stop any in progress decoding. Log.i(TAG, "Setting up video stream decoding."); mediaCodecManager.stopDecoding(new DecoderListener() { @Override @@ -144,8 +148,8 @@ public boolean isLinkConnected() { } private void start(int udpPort, LinkListener listener) { - if(this.linkConn == null || udpPort != this.linkPort){ - if(isStarted.get()){ + if (this.linkConn == null || udpPort != this.linkPort){ + if (isStarted.get()){ stop(); } @@ -211,8 +215,10 @@ public void onIpDisconnected() { @Override public void onPacketReceived(ByteBuffer packetBuffer) { - //Feed this data stream to the decoder. - mediaCodecManager.onInputDataReceived(packetBuffer.array(), packetBuffer.limit()); + if (!videoStreamObserverUsed) { + // Feed this data stream to the decoder. + mediaCodecManager.onInputDataReceived(packetBuffer.array(), packetBuffer.limit()); + } } protected void postSuccessEvent(final ICommandListener listener) { @@ -283,28 +289,29 @@ private void checkForLocalRecording(String appId, Bundle videoProps){ enableLocalRecording(localRecordingFilename); } } - else{ + else { disableLocalRecording(); } } - public void startVideoStream(Bundle videoProps, String appId, String newVideoTag, Surface videoSurface, final ICommandListener listener){ + public void startVideoStream(Bundle videoProps, String appId, String newVideoTag, Surface videoSurface, + final AbstractCommandListener listener) { Timber.d("Video stream start request from %s. Video owner is %s.", appId, videoOwnerId.get()); - if(TextUtils.isEmpty(appId)){ + if (TextUtils.isEmpty(appId)){ postErrorEvent(CommandExecutionError.COMMAND_DENIED, listener); return; } final int udpPort = videoProps.getInt(CameraActions.EXTRA_VIDEO_PROPS_UDP_PORT, -1); - if(videoSurface == null || udpPort == -1){ + if (videoSurface == null || udpPort == -1){ postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); return; } - if(newVideoTag == null) + if (newVideoTag == null) newVideoTag = ""; - if(appId.equals(videoOwnerId.get())){ + if (appId.equals(videoOwnerId.get())) { String currentVideoTag = videoTagRef.get(); if(currentVideoTag == null) currentVideoTag = ""; @@ -318,7 +325,7 @@ public void startVideoStream(Bundle videoProps, String appId, String newVideoTag } } - if (videoOwnerId.compareAndSet(NO_VIDEO_OWNER, appId)){ + if (videoOwnerId.compareAndSet(NO_VIDEO_OWNER, appId)) { videoTagRef.set(newVideoTag); checkForLocalRecording(appId, videoProps); @@ -351,16 +358,64 @@ public void onDecodingEnded() { } } - public void stopVideoStream(String appId, String currentVideoTag, final ICommandListener listener){ + public void startVideoStream(Bundle videoProps, String appId, String newVideoTag, + final VideoStreamListener listener) { + Timber.d("Video stream start request from %s. Video owner is %s.", appId, videoOwnerId.get()); + if (TextUtils.isEmpty(appId)){ + postErrorEvent(CommandExecutionError.COMMAND_DENIED, listener); + return; + } + + final int udpPort = videoProps.getInt(CameraActions.EXTRA_VIDEO_PROPS_UDP_PORT, -1); + if (udpPort == -1){ + postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); + return; + } + + if (newVideoTag == null) + newVideoTag = ""; + + if (appId.equals(videoOwnerId.get())) { + String currentVideoTag = videoTagRef.get(); + if(currentVideoTag == null) + currentVideoTag = ""; + + if (newVideoTag.equals(currentVideoTag)){ + //Check if the local recording state needs to be updated. + checkForLocalRecording(appId, videoProps); + + postSuccessEvent(listener); + return; + } + } + + if (videoOwnerId.compareAndSet(NO_VIDEO_OWNER, appId)) { + videoTagRef.set(newVideoTag); + + checkForLocalRecording(appId, videoProps); + + Timber.i("Start using video observer."); + videoStreamObserverUsed = true; + + listener.onVideoStarted(); + + } + else{ + postErrorEvent(CommandExecutionError.COMMAND_DENIED, listener); + } + } + + public void stopVideoStream(String appId, String currentVideoTag, final AbstractCommandListener listener) { Timber.d("Video stream stop request from %s. Video owner is %s.", appId, videoOwnerId.get()); - if(TextUtils.isEmpty(appId)){ + if (TextUtils.isEmpty(appId)) { Timber.w("Owner id is empty."); postErrorEvent(CommandExecutionError.COMMAND_DENIED, listener); return; } + final String currentVideoOwner = videoOwnerId.get(); - if(NO_VIDEO_OWNER.equals(currentVideoOwner)){ + if (NO_VIDEO_OWNER.equals(currentVideoOwner)) { Timber.d("No video owner set. Nothing to do."); disableLocalRecording(); @@ -369,10 +424,10 @@ public void stopVideoStream(String appId, String currentVideoTag, final ICommand return; } - if(currentVideoTag == null) + if (currentVideoTag == null) currentVideoTag = ""; - if(appId.equals(currentVideoOwner) && currentVideoTag.equals(videoTagRef.get()) + if (appId.equals(currentVideoOwner) && currentVideoTag.equals(videoTagRef.get()) && videoOwnerId.compareAndSet(currentVideoOwner, NO_VIDEO_OWNER)){ videoTagRef.set(""); @@ -383,9 +438,7 @@ public void stopVideoStream(String appId, String currentVideoTag, final ICommand Timber.i("Stopping video decoding."); stopDecoding(new DecoderListener() { @Override - public void onDecodingStarted() { - - } + public void onDecodingStarted() { } @Override public void onDecodingError() { @@ -398,20 +451,59 @@ public void onDecodingEnded() { } }); } - else{ + else { + postErrorEvent(CommandExecutionError.COMMAND_DENIED, listener); + } + } + + public void stopVideoStream(String appId, String currentVideoTag, final VideoStreamListener listener) { + Timber.d("Video stream stop request from %s. Video owner is %s.", appId, videoOwnerId.get()); + if (TextUtils.isEmpty(appId)) { + Timber.w("Owner id is empty."); + postErrorEvent(CommandExecutionError.COMMAND_DENIED, listener); + return; + } + + final String currentVideoOwner = videoOwnerId.get(); + if (NO_VIDEO_OWNER.equals(currentVideoOwner)) { + Timber.d("No video owner set. Nothing to do."); + + disableLocalRecording(); + + postSuccessEvent(listener); + return; + } + + if (currentVideoTag == null) + currentVideoTag = ""; + + if (appId.equals(currentVideoOwner) && currentVideoTag.equals(videoTagRef.get()) + && videoOwnerId.compareAndSet(currentVideoOwner, NO_VIDEO_OWNER)){ + videoTagRef.set(""); + + disableLocalRecording(); + + Timber.d("Stopping video decoding. Current owner is %s.", currentVideoOwner); + + Timber.i("Start using video observer."); + videoStreamObserverUsed = false; + + listener.onVideoStopped(); + } + else { postErrorEvent(CommandExecutionError.COMMAND_DENIED, listener); } } - public void tryStoppingVideoStream(String parentId){ - if(TextUtils.isEmpty(parentId)) + public void tryStoppingVideoStream(String parentId) { + if (TextUtils.isEmpty(parentId)) return; final String videoOwner = videoOwnerId.get(); - if(NO_VIDEO_OWNER.equals(videoOwner)) + if (NO_VIDEO_OWNER.equals(videoOwner)) return; - if(videoOwner.equals(parentId)){ + if (videoOwner.equals(parentId)){ Timber.d("Stopping video owned by %s", parentId); stopVideoStream(parentId, videoTagRef.get(), null); } diff --git a/build.gradle b/build.gradle index 14fb6c6ed5..f4739af6ce 100644 --- a/build.gradle +++ b/build.gradle @@ -18,7 +18,7 @@ buildscript { } dependencies { - classpath 'com.android.tools.build:gradle:2.0.0-alpha3' + classpath 'com.android.tools.build:gradle:1.5.0' classpath 'com.jfrog.bintray.gradle:gradle-bintray-plugin:1.2' classpath 'io.fabric.tools:gradle:1.21.2' } diff --git a/samples/StarterApp/build.gradle b/samples/StarterApp/build.gradle index 6978652388..ceb0df838f 100644 --- a/samples/StarterApp/build.gradle +++ b/samples/StarterApp/build.gradle @@ -23,9 +23,9 @@ android { abortOnError false } - dexOptions { - dexInProcess = true - } + //dexOptions { + // dexInProcess = true + //} } diff --git a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java index 6858c9b284..3a69bd26c3 100644 --- a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java +++ b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java @@ -4,6 +4,7 @@ import android.graphics.SurfaceTexture; import android.os.Bundle; import android.os.Handler; +import android.os.HandlerThread; import android.support.v7.app.AppCompatActivity; import android.util.Log; import android.view.Surface; @@ -18,7 +19,10 @@ import com.o3dr.android.client.ControlTower; import com.o3dr.android.client.Drone; +import com.o3dr.android.client.IVideoStreamCallback; +import com.o3dr.android.client.VideoStreamObserver; import com.o3dr.android.client.apis.ControlApi; +import com.o3dr.android.client.apis.ExperimentalApi; import com.o3dr.android.client.apis.solo.SoloCameraApi; import com.o3dr.android.client.apis.VehicleApi; import com.o3dr.android.client.interfaces.DroneListener; @@ -41,6 +45,7 @@ import com.o3dr.services.android.lib.drone.property.VehicleMode; import com.o3dr.services.android.lib.model.AbstractCommandListener; import com.o3dr.services.android.lib.model.SimpleCommandListener; +import com.o3dr.services.android.lib.model.VideoStreamListener; import java.util.List; @@ -61,6 +66,8 @@ public class MainActivity extends AppCompatActivity implements DroneListener, To private Button startVideoStream; private Button stopVideoStream; + VideoStreamObserver videoStreamObserver; + @Override protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); @@ -124,13 +131,23 @@ public void onSurfaceTextureUpdated(SurfaceTexture surface) { } }); + HandlerThread thread = new HandlerThread("MyHandlerThread"); + thread.start(); + Handler handler = new Handler(thread.getLooper()); + videoStreamObserver = new VideoStreamObserver(this, handler, new IVideoStreamCallback() { + @Override + public void getVideoStreamPackets(byte[] packets) { + alertUser("Video packet received. Size is: " + packets.length); + } + }); + startVideoStream = (Button) findViewById(R.id.start_video_stream); startVideoStream.setEnabled(false); startVideoStream.setOnClickListener(new View.OnClickListener() { @Override public void onClick(View v) { alertUser("Starting video stream."); - startVideoStream(new Surface(videoView.getSurfaceTexture())); + //startVideoStream(new Surface(videoView.getSurfaceTexture())); } }); @@ -140,7 +157,7 @@ public void onClick(View v) { @Override public void onClick(View v) { alertUser("Stopping video stream."); - stopVideoStream(); + //stopVideoStream(); } }); } @@ -495,10 +512,44 @@ private void startVideoStream(Surface videoSurface){ SoloCameraApi.getApi(drone).startVideoStream(videoSurface, "", true, new AbstractCommandListener() { @Override public void onSuccess() { - if(stopVideoStream != null) + if (stopVideoStream != null) stopVideoStream.setEnabled(true); - if(startVideoStream != null) + if (startVideoStream != null) + startVideoStream.setEnabled(false); + } + + @Override + public void onError(int executionError) { + alertUser("Error while starting the video stream: " + executionError); + } + + @Override + public void onTimeout() { + alertUser("Timed out while attempting to start the video stream."); + } + }); + } + + private void startVideoStreamForObserver() { + ExperimentalApi.getApi(drone).startVideoStream("", true, + new VideoStreamListener() { + @Override + public void onVideoStarted() { + alertUser("Video streaming has started..."); + } + + @Override + public void onVideoStopped() { + alertUser("Video streaming has stopped..."); + } + + @Override + public void onSuccess() { + if (stopVideoStream != null) + stopVideoStream.setEnabled(true); + + if (startVideoStream != null) startVideoStream.setEnabled(false); } @@ -515,16 +566,53 @@ public void onTimeout() { } private void stopVideoStream() { - SoloCameraApi.getApi(drone).stopVideoStream(new SimpleCommandListener() { + SoloCameraApi.getApi(drone).stopVideoStream(new AbstractCommandListener() { @Override public void onSuccess() { if (stopVideoStream != null) stopVideoStream.setEnabled(false); - if(startVideoStream != null) + if (startVideoStream != null) startVideoStream.setEnabled(true); } + + @Override + public void onError(int executionError) { + } + + @Override + public void onTimeout() { + } }); } + private void stopVideoStreamForObserver() { + ExperimentalApi.getApi(drone).stopVideoStream("", + new VideoStreamListener() { + @Override + public void onVideoStarted() { + alertUser("Video streaming has started..."); + } + + @Override + public void onVideoStopped() { + alertUser("Video streaming has stopped..."); + } + + @Override + public void onSuccess() { + if (stopVideoStream != null) + stopVideoStream.setEnabled(false); + + if (startVideoStream != null) + startVideoStream.setEnabled(true); + } + + @Override + public void onError(int executionError) { } + + @Override + public void onTimeout() { } + }); + } } From 8e3e97ffb5569f3e688a7d8506c95678fabbf81f Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Mon, 8 Feb 2016 19:43:05 -0800 Subject: [PATCH 02/37] First commit to refactor link state callbacks --- .../java/com/o3dr/android/client/Drone.java | 39 ++- .../o3dr/android/client/DroneObserver.java | 1 - .../client/interfaces/DroneListener.java | 4 + .../client/interfaces/LinkListener.java | 11 + .../drone/connection/ConnectionResult.java | 1 + .../connection/LinkConnectionStatus.java | 123 ++++++++ .../services/android/lib/link/LinkEvent.java | 15 + .../android/lib/link/LinkEventExtra.java | 13 + .../android/lib/model/IApiListener.aidl | 1 + .../android/api/DroidPlannerService.java | 7 +- .../services/android/api/DroneApi.java | 128 +++++---- .../connection/AndroidMavLinkConnection.java | 2 - .../connection/AndroidTcpConnection.java | 105 ++++--- .../connection/AndroidUdpConnection.java | 6 - .../connection/BluetoothConnection.java | 269 +++++++++--------- .../connection/SoloConnection.java | 40 ++- .../connection/usb/UsbCDCConnection.java | 17 +- .../connection/usb/UsbConnection.java | 257 ++++++++--------- .../android/communication/model/DataLink.java | 8 +- .../communication/service/MAVLinkClient.java | 39 ++- .../MAVLink/connection/MavLinkConnection.java | 98 ++++--- .../connection/MavLinkConnectionListener.java | 39 +-- .../MAVLink/connection/TcpConnection.java | 89 +++--- .../MAVLink/connection/UdpConnection.java | 147 +++++----- .../android/core/drone/DroneManager.java | 98 ++++--- .../drone/manager/MavLinkDroneManager.java | 27 +- .../connection/WifiConnectionHandler.java | 24 +- .../o3dr/sample/hellodrone/MainActivity.java | 2 +- 28 files changed, 923 insertions(+), 687 deletions(-) create mode 100644 ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java create mode 100644 ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java create mode 100644 ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEvent.java create mode 100644 ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEventExtra.java 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 f8f73bf186..77b807305c 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/Drone.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/Drone.java @@ -16,16 +16,20 @@ import com.o3dr.android.client.apis.MissionApi; import com.o3dr.android.client.apis.VehicleApi; import com.o3dr.android.client.interfaces.DroneListener; +import com.o3dr.android.client.interfaces.LinkListener; import com.o3dr.android.client.utils.TxPowerComplianceCountries; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.drone.attribute.AttributeEvent; import com.o3dr.services.android.lib.drone.attribute.AttributeType; +import com.o3dr.services.android.lib.link.LinkEvent; +import com.o3dr.services.android.lib.link.LinkEventExtra; import com.o3dr.services.android.lib.drone.calibration.magnetometer.MagnetometerCalibrationStatus; import com.o3dr.services.android.lib.drone.companion.solo.SoloAttributes; import com.o3dr.services.android.lib.drone.companion.solo.SoloEventExtras; import com.o3dr.services.android.lib.drone.companion.solo.SoloEvents; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.ConnectionResult; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.mission.Mission; import com.o3dr.services.android.lib.drone.mission.item.MissionItem; import com.o3dr.services.android.lib.drone.property.Altitude; @@ -108,6 +112,7 @@ public void binderDied() { private final AtomicReference droneApiRef = new AtomicReference<>(null); private ConnectionParameter connectionParameter; + private LinkListener linkListener; private ExecutorService asyncScheduler; // flightTimer @@ -392,13 +397,19 @@ private T getAttributeDefaultValue(String attributeType) } public void connect(final ConnectionParameter connParams) { + connect(connParams, null); + } + + public void connect(ConnectionParameter connParams, LinkListener linkListener) { VehicleApi.getApi(this).connect(connParams); this.connectionParameter = connParams; + this.linkListener = linkListener; } public void disconnect() { VehicleApi.getApi(this).disconnect(); this.connectionParameter = null; + this.linkListener = null; } private static AbstractCommandListener wrapListener(final Handler handler, final AbstractCommandListener listener) { @@ -761,14 +772,18 @@ public void onRetrievalFailed() { final Bundle eventInfo = new Bundle(1); boolean isEUCompliant = !TxPowerComplianceCountries.getDefaultCountry().name().equals(compliantCountry); eventInfo.putBoolean(SoloEventExtras.EXTRA_SOLO_EU_TX_POWER_COMPLIANT, isEUCompliant); - sendEventToListeners(SoloEvents.SOLO_EU_TX_POWER_COMPLIANCE_UPDATED, eventInfo); + sendAttributeEventToListener(SoloEvents.SOLO_EU_TX_POWER_COMPLIANCE_UPDATED, eventInfo); break; + + case LinkEvent.LINK_STATE_UPDATED: + sendLinkEventToListener(extras); + return; } - sendEventToListeners(attributeEvent, extras); + sendAttributeEventToListener(attributeEvent, extras); } - private void sendEventToListeners(final String attributeEvent, final Bundle extras) { + private void sendAttributeEventToListener(final String attributeEvent, final Bundle extras) { if (droneListeners.isEmpty()) { return; } @@ -787,6 +802,24 @@ public void run() { }); } + private void sendLinkEventToListener(final Bundle extras) { + if (linkListener == null) { + return; + } + + handler.post(new Runnable() { + @Override + public void run() { + LinkConnectionStatus status = null; + if (extras != null) { + status = extras.getParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS); + } + + linkListener.onLinkStateUpdated(status); + } + }); + } + void notifyDroneServiceInterrupted(final String errorMsg) { if (droneListeners.isEmpty()) { return; diff --git a/ClientLib/src/main/java/com/o3dr/android/client/DroneObserver.java b/ClientLib/src/main/java/com/o3dr/android/client/DroneObserver.java index ab301dc2fa..3e85011a68 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/DroneObserver.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/DroneObserver.java @@ -3,7 +3,6 @@ import android.os.Bundle; import android.os.RemoteException; -import com.o3dr.services.android.lib.drone.connection.ConnectionResult; import com.o3dr.services.android.lib.model.IObserver; /** diff --git a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java index 7eb83fbb2e..f312c1676d 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java @@ -9,6 +9,10 @@ */ public interface DroneListener { + /** + * @deprecated + * @param result + */ void onDroneConnectionFailed(ConnectionResult result); void onDroneEvent(String event, Bundle extras); diff --git a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java new file mode 100644 index 0000000000..2d46119350 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java @@ -0,0 +1,11 @@ +package com.o3dr.android.client.interfaces; + +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; + +/** + * Created by chavi on 2/8/16. + */ +public interface LinkListener { + + void onLinkStateUpdated(LinkConnectionStatus connectionStatus); +} diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java index ddc5912c06..770fb250c7 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java @@ -4,6 +4,7 @@ import android.os.Parcelable; /** + * @deprecated * Conveys information if the connection attempt fails. */ public final class ConnectionResult implements Parcelable { diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java new file mode 100644 index 0000000000..3ce5427072 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java @@ -0,0 +1,123 @@ +package com.o3dr.services.android.lib.drone.connection; + +import android.os.Bundle; +import android.os.Parcel; +import android.os.Parcelable; +import android.support.annotation.IntDef; +import android.support.annotation.StringDef; + +/** + * Conveys information if the connection attempt fails. + */ +public final class LinkConnectionStatus implements Parcelable { + @StringDef({ + CONNECTED, + CONNECTING, + DISCONNECTED, + FAILED + }) + public @interface StatusCode{} + public static final String CONNECTED = "CONNECTED"; + public static final String CONNECTING = "CONNECTING"; + public static final String DISCONNECTED = "DISCONNECTED"; + public static final String FAILED = "FAILED"; + + public static final String EXTRA_ERROR_CODE_KEY = "extra_error_code"; + public static final String EXTRA_ERROR_MSG_KEY = "extra_error_message"; + public static final String EXTRA_CONNECTION_TIME = "extra_connection_time"; + + @IntDef({ + SYSTEM_UNAVAILABLE, + LINK_UNAVAILABLE, + PERMISSION_DENIED, + INVALID_CREDENTIALS, + TIMEOUT, + ADDRESS_IN_USE, + UNKNOWN + }) + public @interface ErrorCode{} + public static final int SYSTEM_UNAVAILABLE = -1; + public static final int LINK_UNAVAILABLE = -2; + public static final int PERMISSION_DENIED = -3; + public static final int INVALID_CREDENTIALS = -4; + public static final int TIMEOUT = -5; + public static final int ADDRESS_IN_USE = -6; + public static final int UNKNOWN = -7; + + private final @StatusCode String mStatusCode; + private final Bundle mExtras; + + public LinkConnectionStatus(@StatusCode String errorCode, Bundle extras) { + this.mStatusCode = errorCode; + this.mExtras = extras; + } + + public @StatusCode String getStatusCode() { + return mStatusCode; + } + + public Bundle getExtras() { + return mExtras; + } + + @Override + public int describeContents() { + return 0; + } + + @Override + public void writeToParcel(Parcel dest, int flags) { + dest.writeString(this.mStatusCode); + dest.writeBundle(this.mExtras); + } + + private LinkConnectionStatus(Parcel in) { + @StatusCode String statusCode = in.readString(); + + this.mStatusCode = statusCode; + this.mExtras = in.readBundle(); + } + + public static final Parcelable.Creator CREATOR = new Parcelable.Creator() { + public LinkConnectionStatus createFromParcel(Parcel source) { + return new LinkConnectionStatus(source); + } + + public LinkConnectionStatus[] newArray(int size) { + return new LinkConnectionStatus[size]; + } + }; + + @Override + public boolean equals(Object o) { + if (this == o) { + return true; + } + if (o == null || getClass() != o.getClass()) { + return false; + } + + LinkConnectionStatus that = (LinkConnectionStatus) o; + + if (mStatusCode != null ? !mStatusCode.equals(that.mStatusCode) : that.mStatusCode != null) { + return false; + } + return !(mExtras != null ? !mExtras.equals(that.mExtras) : that.mExtras != null); + + } + + @Override + public int hashCode() { + int result = mStatusCode != null ? mStatusCode.hashCode() : 0; + result = 31 * result + (mExtras != null ? mExtras.hashCode() : 0); + return result; + } + + @Override + public String toString() { + return "ConnectionResult{" + + "mStatusCode='" + mStatusCode + '\'' + + ", mExtras=" + mExtras + + '}'; + } +} diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEvent.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEvent.java new file mode 100644 index 0000000000..fdd4927ce1 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEvent.java @@ -0,0 +1,15 @@ +package com.o3dr.services.android.lib.link; + +/** + * Created by chavi on 2/5/16. + */ +public class LinkEvent { + + private LinkEvent() { + } + + private static final String PACKAGE_NAME = "com.o3dr.services.android.lib.link.event"; + + public static final String LINK_STATE_UPDATED = PACKAGE_NAME + ".LINK_STATE_UPDATED"; + +} diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEventExtra.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEventExtra.java new file mode 100644 index 0000000000..f625110f97 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEventExtra.java @@ -0,0 +1,13 @@ +package com.o3dr.services.android.lib.link; + +/** + * Created by chavi on 2/5/16. + */ +public class LinkEventExtra { + private LinkEventExtra() { + } + + private static final String PACKAGE_NAME = "com.o3dr.services.android.lib.link.event.extra"; + + public static final String EXTRA_CONNECTION_STATUS = PACKAGE_NAME + ".CONNECTION_STATUS"; +} diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/model/IApiListener.aidl b/ClientLib/src/main/java/com/o3dr/services/android/lib/model/IApiListener.aidl index 6c4637be52..ef7a544a45 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/model/IApiListener.aidl +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/model/IApiListener.aidl @@ -13,6 +13,7 @@ interface IApiListener { int getApiVersionCode(); /** + * @deprecated * Called when the connection attempt fails. * @param result Describe why the connection failed. */ diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroidPlannerService.java b/ServiceApp/src/org/droidplanner/services/android/api/DroidPlannerService.java index 5c11a7f0e7..b5a47d0ef7 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroidPlannerService.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroidPlannerService.java @@ -22,7 +22,6 @@ import org.droidplanner.services.android.R; import org.droidplanner.services.android.core.drone.DroneManager; import org.droidplanner.services.android.core.survey.CameraInfo; -import org.droidplanner.services.android.exception.ConnectionException; import org.droidplanner.services.android.ui.activity.MainActivity; import org.droidplanner.services.android.utils.Utils; import org.droidplanner.services.android.utils.file.IO.CameraInfoLoader; @@ -117,9 +116,8 @@ void releaseDroneApi(String appId) { * @param appId Application id of the connecting client. * @param listener Callback to receive drone events. * @return A DroneManager instance which acts as router between the connected vehicle and the listeneing client(s). - * @throws ConnectionException */ - DroneManager connectDroneManager(ConnectionParameter connParams, String appId, DroneApi listener) throws ConnectionException { + DroneManager connectDroneManager(ConnectionParameter connParams, String appId, DroneApi listener) { if (connParams == null || TextUtils.isEmpty(appId) || listener == null) return null; @@ -147,9 +145,8 @@ DroneManager connectDroneManager(ConnectionParameter connParams, String appId, D * * @param droneMgr Handler for the connected vehicle. * @param clientInfo Info of the disconnecting client. - * @throws ConnectionException */ - void disconnectDroneManager(DroneManager droneMgr, DroneApi.ClientInfo clientInfo) throws ConnectionException { + void disconnectDroneManager(DroneManager droneMgr, DroneApi.ClientInfo clientInfo) { if (droneMgr == null || clientInfo == null || TextUtils.isEmpty(clientInfo.appId)) return; diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index 4cd38ef7bc..1c80207dd1 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -12,15 +12,17 @@ import com.MAVLink.Messages.MAVLinkMessage; import com.MAVLink.ardupilotmega.msg_mag_cal_progress; import com.MAVLink.ardupilotmega.msg_mag_cal_report; -import com.o3dr.services.android.lib.drone.action.CameraActions; import com.o3dr.services.android.lib.coordinate.LatLongAlt; +import com.o3dr.services.android.lib.drone.action.CameraActions; import com.o3dr.services.android.lib.drone.action.ConnectionActions; import com.o3dr.services.android.lib.drone.attribute.AttributeEvent; import com.o3dr.services.android.lib.drone.attribute.AttributeEventExtra; import com.o3dr.services.android.lib.drone.attribute.AttributeType; +import com.o3dr.services.android.lib.link.LinkEvent; import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; +import com.o3dr.services.android.lib.link.LinkEventExtra; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; -import com.o3dr.services.android.lib.drone.connection.ConnectionResult; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.mission.Mission; import com.o3dr.services.android.lib.drone.mission.action.MissionActions; import com.o3dr.services.android.lib.drone.mission.item.MissionItem; @@ -60,7 +62,7 @@ * Implementation for the IDroneApi interface. */ public final class DroneApi extends IDroneApi.Stub implements DroneInterfaces.OnDroneListener, DroneInterfaces.AttributeEventListener, - DroneInterfaces.OnParameterManagerListener, MagnetometerCalibrationImpl.OnMagnetometerCalibrationListener, IBinder.DeathRecipient { + DroneInterfaces.OnParameterManagerListener, MagnetometerCalibrationImpl.OnMagnetometerCalibrationListener, IBinder.DeathRecipient { //The Reset ROI mission item was introduced in version 2.6.8. Any client library older than this do not support it. private final static int RESET_ROI_LIB_VERSION = 206080; @@ -117,11 +119,7 @@ void destroy() { Timber.e(e, e.getMessage()); } - try { - this.service.disconnectDroneManager(this.droneMgr, this.clientInfo); - } catch (ConnectionException e) { - Timber.e(e, e.getMessage()); - } + this.service.disconnectDroneManager(this.droneMgr, this.clientInfo); } public String getOwnerId() { @@ -133,8 +131,9 @@ public DroneManager getDroneManager() { } private Drone getDrone() { - if (this.droneMgr == null) + if (this.droneMgr == null) { return null; + } return this.droneMgr.getDrone(); } @@ -149,19 +148,19 @@ public Bundle getAttribute(String type) throws RemoteException { break; default: - if(droneMgr != null) { + if (droneMgr != null) { DroneAttribute attribute = droneMgr.getAttribute(clientInfo, type); if (attribute != null) { //Check if the client supports the ResetROI mission item. // Replace it with a RegionOfInterest with coordinate set to 0 if it doesn't. - if(clientInfo.clientVersionCode < RESET_ROI_LIB_VERSION && attribute instanceof Mission){ + if (clientInfo.clientVersionCode < RESET_ROI_LIB_VERSION && attribute instanceof Mission) { Mission proxyMission = (Mission) attribute; List missionItems = proxyMission.getMissionItems(); int missionItemsCount = missionItems.size(); - for(int i = 0; i < missionItemsCount; i++){ + for (int i = 0; i < missionItemsCount; i++) { MissionItem missionItem = missionItems.get(i); - if(missionItem instanceof ResetROI){ + if (missionItem instanceof ResetROI) { missionItems.remove(i); RegionOfInterest replacement = new RegionOfInterest(); @@ -184,13 +183,13 @@ public boolean isConnected() { } private ConnectionParameter checkConnectionParameter(ConnectionParameter connParams) throws ConnectionException { - if(connParams == null){ + if (connParams == null) { throw new ConnectionException("Invalid connection parameters"); } - if(SoloConnection.isSoloConnection(context, connParams)){ + if (SoloConnection.isSoloConnection(context, connParams)) { ConnectionParameter update = SoloConnection.getSoloConnectionParameterIfPossible(context); - if(update != null){ + if (update != null) { return update; } } @@ -202,18 +201,18 @@ public void connect(ConnectionParameter connParams) { this.connectionParams = checkConnectionParameter(connParams); this.droneMgr = service.connectDroneManager(this.connectionParams, ownerId, this); } catch (ConnectionException e) { - notifyConnectionFailed(new ConnectionResult(0, e.getMessage())); + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.INVALID_CREDENTIALS); + extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, e.getMessage()); + notifyConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); disconnect(); } } public void disconnect() { - try { - service.disconnectDroneManager(this.droneMgr, clientInfo); - this.droneMgr = null; - } catch (ConnectionException e) { - notifyConnectionFailed(new ConnectionResult(0, e.getMessage())); - } + service.disconnectDroneManager(this.droneMgr, clientInfo); + this.droneMgr = null; + } private void checkForSelfRelease() { @@ -221,8 +220,8 @@ private void checkForSelfRelease() { if (!apiListener.asBinder().pingBinder()) { Timber.w("Client is not longer available."); this.context.startService(new Intent(this.context, DroidPlannerService.class) - .setAction(DroidPlannerService.ACTION_RELEASE_API_INSTANCE) - .putExtra(DroidPlannerService.EXTRA_API_INSTANCE_APP_ID, this.ownerId)); + .setAction(DroidPlannerService.ACTION_RELEASE_API_INSTANCE) + .putExtra(DroidPlannerService.EXTRA_API_INSTANCE_APP_ID, this.ownerId)); } } @@ -245,8 +244,9 @@ public void removeAttributesObserver(IObserver observer) throws RemoteException @Override public void addMavlinkObserver(IMavlinkObserver observer) throws RemoteException { - if (observer != null) + if (observer != null) { mavlinkObserversList.add(observer); + } } @Override @@ -259,16 +259,19 @@ public void removeMavlinkObserver(IMavlinkObserver observer) throws RemoteExcept @Override public void executeAction(Action action, ICommandListener listener) throws RemoteException { - if (action == null) + if (action == null) { return; + } String type = action.getType(); - if (type == null) + if (type == null) { return; + } Bundle data = action.getData(); - if (data != null) + if (data != null) { data.setClassLoader(context.getClassLoader()); + } Drone drone = getDrone(); switch (type) { @@ -308,10 +311,9 @@ public void executeAction(Action action, ICommandListener listener) throws Remot // MISSION ACTIONS case MissionActions.ACTION_BUILD_COMPLEX_MISSION_ITEM: - if(drone instanceof MavLinkDrone) { - CommonApiUtils.buildComplexMissionItem((MavLinkDrone)drone, data); - } - else{ + if (drone instanceof MavLinkDrone) { + CommonApiUtils.buildComplexMissionItem((MavLinkDrone) drone, data); + } else { CommonApiUtils.postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); } break; @@ -342,8 +344,9 @@ public void performAsyncAction(Action action) throws RemoteException { } private void notifyAttributeUpdate(List> attributesInfo) { - if (observersList.isEmpty() || attributesInfo == null || attributesInfo.isEmpty()) + if (observersList.isEmpty() || attributesInfo == null || attributesInfo.isEmpty()) { return; + } for (Pair info : attributesInfo) { notifyAttributeUpdate(info.first, info.second); @@ -351,8 +354,9 @@ private void notifyAttributeUpdate(List> attributesInfo) { } private void notifyAttributeUpdate(String attributeEvent, Bundle extrasBundle) { - if (observersList.isEmpty()) + if (observersList.isEmpty()) { return; + } if (attributeEvent != null) { for (IObserver observer : observersList) { @@ -370,21 +374,16 @@ private void notifyAttributeUpdate(String attributeEvent, Bundle extrasBundle) { } } - private void notifyConnectionFailed(ConnectionResult result) { - if (result != null) { - try { - apiListener.onConnectionFailed(result); - return; - } catch (RemoteException e) { - Timber.w(e, "Unable to forward connection fail to client."); - } - checkForSelfRelease(); - } + private void notifyConnectionStatus(LinkConnectionStatus connectionStatus) { + Bundle extras = new Bundle(); + extras.putParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS, connectionStatus); + notifyAttributeUpdate(LinkEvent.LINK_STATE_UPDATED, extras); } public void onReceivedMavLinkMessage(MAVLinkMessage msg) { - if (mavlinkObserversList.isEmpty()) + if (mavlinkObserversList.isEmpty()) { return; + } if (msg != null) { MavlinkMessageWrapper msgWrapper = new MavlinkMessageWrapper(msg); @@ -416,8 +415,9 @@ public ClientInfo getClientInfo() { @Override public void onAttributeEvent(String attributeEvent, Bundle eventInfo, boolean checkForSololinkApi) { - if (TextUtils.isEmpty(attributeEvent)) + if (TextUtils.isEmpty(attributeEvent)) { return; + } notifyAttributeUpdate(attributeEvent, eventInfo); } @@ -439,7 +439,7 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { case DISCONNECTED: //Broadcast the disconnection with the vehicle. context.sendBroadcast(new Intent(GCSEvent.ACTION_VEHICLE_DISCONNECTION) - .putExtra(GCSEvent.EXTRA_APP_ID, ownerId)); + .putExtra(GCSEvent.EXTRA_APP_ID, ownerId)); droneEvent = AttributeEvent.STATE_DISCONNECTED; break; @@ -464,7 +464,7 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { case AUTOPILOT_WARNING: State droneState = (State) drone.getAttribute(AttributeType.STATE); - if(droneState != null) { + if (droneState != null) { extrasBundle.putString(AttributeEventExtra.EXTRA_AUTOPILOT_ERROR_ID, droneState.getAutopilotErrorId()); } droneEvent = AttributeEvent.AUTOPILOT_ERROR; @@ -509,7 +509,7 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { break; case CALIBRATION_IMU: - if(drone instanceof MavLinkDrone) { + if (drone instanceof MavLinkDrone) { String calIMUMessage = ((MavLinkDrone) drone).getCalibrationSetup().getMessage(); extrasBundle.putString(AttributeEventExtra.EXTRA_CALIBRATION_IMU_MESSAGE, calIMUMessage); droneEvent = AttributeEvent.CALIBRATION_IMU; @@ -517,7 +517,7 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { break; case CALIBRATION_TIMEOUT: - if(drone instanceof MavLinkDrone) { + if (drone instanceof MavLinkDrone) { /* * here we will check if we are in calibration mode but if at * the same time 'msg' is empty - then it is actually not doing @@ -547,13 +547,15 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { case CONNECTION_FAILED: disconnect(); - onConnectionFailed(""); + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.UNKNOWN); + onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); break; case HEARTBEAT_FIRST: Bundle heartBeatExtras = new Bundle(); heartBeatExtras.putString(AttributeEventExtra.EXTRA_VEHICLE_ID, drone.getId()); - if(drone instanceof MavLinkDrone) { + if (drone instanceof MavLinkDrone) { heartBeatExtras.putInt(AttributeEventExtra.EXTRA_MAVLINK_VERSION, ((MavLinkDrone) drone).getMavlinkVersion()); } attributesInfo.add(Pair.create(AttributeEvent.HEARTBEAT_FIRST, heartBeatExtras)); @@ -561,17 +563,17 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { case CONNECTED: //Broadcast the vehicle connection. ConnectionParameter sanitizedParameter = new ConnectionParameter(connectionParams - .getConnectionType(), connectionParams.getParamsBundle()); + .getConnectionType(), connectionParams.getParamsBundle()); context.sendBroadcast(new Intent(GCSEvent.ACTION_VEHICLE_CONNECTION) - .putExtra(GCSEvent.EXTRA_APP_ID, ownerId) - .putExtra(GCSEvent.EXTRA_VEHICLE_CONNECTION_PARAMETER, sanitizedParameter)); + .putExtra(GCSEvent.EXTRA_APP_ID, ownerId) + .putExtra(GCSEvent.EXTRA_VEHICLE_CONNECTION_PARAMETER, sanitizedParameter)); attributesInfo.add(Pair.create(AttributeEvent.STATE_CONNECTED, extrasBundle)); break; case HEARTBEAT_RESTORED: - if(drone instanceof MavLinkDrone) { + if (drone instanceof MavLinkDrone) { extrasBundle.putInt(AttributeEventExtra.EXTRA_MAVLINK_VERSION, ((MavLinkDrone) drone).getMavlinkVersion()); } droneEvent = AttributeEvent.HEARTBEAT_RESTORED; @@ -585,7 +587,7 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { break; case MISSION_WP_UPDATE: - if(drone instanceof MavLinkDrone) { + if (drone instanceof MavLinkDrone) { int currentWaypoint = ((MavLinkDrone) drone).getMissionStats().getCurrentWP(); extrasBundle.putInt(AttributeEventExtra.EXTRA_MISSION_CURRENT_WAYPOINT, currentWaypoint); droneEvent = AttributeEvent.MISSION_ITEM_UPDATED; @@ -593,7 +595,7 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { break; case MISSION_WP_REACHED: - if(drone instanceof MavLinkDrone) { + if (drone instanceof MavLinkDrone) { int lastReachedWaypoint = ((MavLinkDrone) drone).getMissionStats().getLastReachedWP(); extrasBundle.putInt(AttributeEventExtra.EXTRA_MISSION_LAST_REACHED_WAYPOINT, lastReachedWaypoint); droneEvent = AttributeEvent.MISSION_ITEM_REACHED; @@ -657,8 +659,8 @@ public void onEndReceivingParameters() { notifyAttributeUpdate(AttributeEvent.PARAMETERS_REFRESH_COMPLETED, null); } - public void onConnectionFailed(String error) { - notifyConnectionFailed(new ConnectionResult(0, error)); + public void onConnectionStatus(LinkConnectionStatus connectionStatus) { + notifyConnectionStatus(connectionStatus); } @Override @@ -675,7 +677,7 @@ public void onCalibrationCancelled() { public void onCalibrationProgress(msg_mag_cal_progress progress) { Bundle progressBundle = new Bundle(1); progressBundle.putParcelable(AttributeEventExtra.EXTRA_CALIBRATION_MAG_PROGRESS, - CommonApiUtils.getMagnetometerCalibrationProgress(progress)); + CommonApiUtils.getMagnetometerCalibrationProgress(progress)); notifyAttributeUpdate(AttributeEvent.CALIBRATION_MAG_PROGRESS, progressBundle); } @@ -684,7 +686,7 @@ public void onCalibrationProgress(msg_mag_cal_progress progress) { public void onCalibrationCompleted(msg_mag_cal_report report) { Bundle reportBundle = new Bundle(1); reportBundle.putParcelable(AttributeEventExtra.EXTRA_CALIBRATION_MAG_RESULT, - CommonApiUtils.getMagnetometerCalibrationResult(report)); + CommonApiUtils.getMagnetometerCalibrationResult(report)); notifyAttributeUpdate(AttributeEvent.CALIBRATION_MAG_COMPLETED, reportBundle); } diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidMavLinkConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidMavLinkConnection.java index a0bd2abae9..3e8d35bd25 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidMavLinkConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidMavLinkConnection.java @@ -8,8 +8,6 @@ public abstract class AndroidMavLinkConnection extends MavLinkConnection { - private static final String TAG = AndroidMavLinkConnection.class.getSimpleName(); - protected final Context mContext; public AndroidMavLinkConnection(Context applicationContext) { diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidTcpConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidTcpConnection.java index e613511827..13170af77d 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidTcpConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidTcpConnection.java @@ -1,78 +1,73 @@ package org.droidplanner.services.android.communication.connection; -import java.io.IOException; +import android.content.Context; import org.droidplanner.services.android.core.MAVLink.connection.TcpConnection; import org.droidplanner.services.android.core.model.Logger; -import android.content.Context; +import java.io.IOException; public class AndroidTcpConnection extends AndroidMavLinkConnection { - private final TcpConnection mConnectionImpl; + private final TcpConnection mConnectionImpl; private final String serverIp; private final int serverPort; - public AndroidTcpConnection(Context context, String tcpServerIp, int tcpServerPort) { - super(context); + public AndroidTcpConnection(Context context, String tcpServerIp, int tcpServerPort) { + super(context); this.serverIp = tcpServerIp; this.serverPort = tcpServerPort; - mConnectionImpl = new TcpConnection() { - @Override - protected int loadServerPort() { - return serverPort; - } - - @Override - protected String loadServerIP() { - return serverIp; - } + mConnectionImpl = new TcpConnection() { + @Override + protected int loadServerPort() { + return serverPort; + } - @Override - protected Logger initLogger() { - return AndroidTcpConnection.this.initLogger(); - } + @Override + protected String loadServerIP() { + return serverIp; + } @Override - protected void onConnectionOpened(){ - AndroidTcpConnection.this.onConnectionOpened(); + protected Logger initLogger() { + return AndroidTcpConnection.this.initLogger(); } @Override - protected void onConnectionFailed(String errMsg){ - AndroidTcpConnection.this.onConnectionFailed(errMsg); + protected void onConnectionOpened() { + AndroidTcpConnection.this.onConnectionOpened(); } - }; - } - - @Override - protected void closeConnection() throws IOException { - mConnectionImpl.closeConnection(); - } - - @Override - protected void loadPreferences() { - mConnectionImpl.loadPreferences(); - } - - @Override - protected void openConnection() throws IOException { - mConnectionImpl.openConnection(); - } - - @Override - protected int readDataBlock(byte[] buffer) throws IOException { - return mConnectionImpl.readDataBlock(buffer); - } - - @Override - protected void sendBuffer(byte[] buffer) throws IOException { - mConnectionImpl.sendBuffer(buffer); - } - - @Override - public int getConnectionType() { - return mConnectionImpl.getConnectionType(); - } + }; + } + + @Override + protected void closeConnection() throws IOException { + mConnectionImpl.closeConnection(); + } + + @Override + protected void loadPreferences() { + mConnectionImpl.loadPreferences(); + } + + @Override + protected void openConnection() throws IOException { + mConnectionImpl.openConnection(); + } + + @Override + protected int readDataBlock(byte[] buffer) throws IOException { + return mConnectionImpl.readDataBlock(buffer); + } + + @Override + protected void sendBuffer(byte[] buffer) throws IOException { + mConnectionImpl.sendBuffer(buffer); + } + + @Override + public int getConnectionType() { + return mConnectionImpl.getConnectionType(); + } } diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidUdpConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidUdpConnection.java index aa142bb95b..788811fdfc 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidUdpConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidUdpConnection.java @@ -43,12 +43,6 @@ protected Logger initLogger() { protected void onConnectionOpened() { AndroidUdpConnection.this.onConnectionOpened(); } - - @Override - protected void onConnectionFailed(String errMsg) { - AndroidUdpConnection.this.onConnectionFailed(errMsg); - } - }; } diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/BluetoothConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/BluetoothConnection.java index 5299b037f4..99ddfd4ba1 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/BluetoothConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/BluetoothConnection.java @@ -1,14 +1,5 @@ package org.droidplanner.services.android.communication.connection; -import java.io.IOException; -import java.io.InputStream; -import java.io.OutputStream; -import java.net.UnknownHostException; -import java.util.Set; -import java.util.UUID; - -import org.droidplanner.services.android.core.MAVLink.connection.MavLinkConnectionTypes; - import android.annotation.SuppressLint; import android.bluetooth.BluetoothAdapter; import android.bluetooth.BluetoothDevice; @@ -17,137 +8,147 @@ import android.os.ParcelUuid; import android.util.Log; +import org.droidplanner.services.android.core.MAVLink.connection.MavLinkConnectionTypes; + +import java.io.IOException; +import java.io.InputStream; +import java.io.OutputStream; +import java.net.UnknownHostException; +import java.util.Set; +import java.util.UUID; + public class BluetoothConnection extends AndroidMavLinkConnection { - private static final String BLUE = "BLUETOOTH"; - private static final String UUID_SPP_DEVICE = "00001101-0000-1000-8000-00805F9B34FB"; - private BluetoothAdapter mBluetoothAdapter; - private OutputStream out; - private InputStream in; - private BluetoothSocket bluetoothSocket; + private static final String BLUE = "BLUETOOTH"; + private static final String UUID_SPP_DEVICE = "00001101-0000-1000-8000-00805F9B34FB"; + private BluetoothAdapter mBluetoothAdapter; + private OutputStream out; + private InputStream in; + private BluetoothSocket bluetoothSocket; private final String bluetoothAddress; - public BluetoothConnection(Context parentContext, String btAddress) { - super(parentContext); + public BluetoothConnection(Context parentContext, String btAddress) { + super(parentContext); this.bluetoothAddress = btAddress; - mBluetoothAdapter = BluetoothAdapter.getDefaultAdapter(); - if (mBluetoothAdapter == null) { - Log.d(BLUE, "Null adapters"); - } - } - - @Override - protected void openConnection() throws IOException { - Log.d(BLUE, "Connect"); - - // Reset the bluetooth connection - resetConnection(); - - // Retrieve the stored device - BluetoothDevice device = null; - try { - device = mBluetoothAdapter.getRemoteDevice(bluetoothAddress); - } catch (IllegalArgumentException ex) { - // invalid configuration (device may have been removed) - // NOP fall through to 'no device' - } - - // no device - if (device == null) - device = findSerialBluetoothBoard(); - - Log.d(BLUE, "Trying to connect to device with address " + device.getAddress()); - Log.d(BLUE, "BT Create Socket Call..."); - bluetoothSocket = device.createInsecureRfcommSocketToServiceRecord(UUID - .fromString(UUID_SPP_DEVICE)); - - Log.d(BLUE, "BT Cancel Discovery Call..."); - mBluetoothAdapter.cancelDiscovery(); - - Log.d(BLUE, "BT Connect Call..."); - bluetoothSocket.connect(); // Here the IOException will rise on BT - // protocol/handshake error. - - Log.d(BLUE, "## BT Connected ##"); - out = bluetoothSocket.getOutputStream(); - in = bluetoothSocket.getInputStream(); + mBluetoothAdapter = BluetoothAdapter.getDefaultAdapter(); + if (mBluetoothAdapter == null) { + Log.d(BLUE, "Null adapters"); + } + } + + @Override + protected void openConnection() throws IOException { + Log.d(BLUE, "Connect"); + + // Reset the bluetooth connection + resetConnection(); + + // Retrieve the stored device + BluetoothDevice device = null; + try { + device = mBluetoothAdapter.getRemoteDevice(bluetoothAddress); + } catch (IllegalArgumentException ex) { + // invalid configuration (device may have been removed) + // NOP fall through to 'no device' + } + + // no device + if (device == null) { + device = findSerialBluetoothBoard(); + } + + Log.d(BLUE, "Trying to connect to device with address " + device.getAddress()); + Log.d(BLUE, "BT Create Socket Call..."); + bluetoothSocket = device.createInsecureRfcommSocketToServiceRecord(UUID + .fromString(UUID_SPP_DEVICE)); + + Log.d(BLUE, "BT Cancel Discovery Call..."); + mBluetoothAdapter.cancelDiscovery(); + + Log.d(BLUE, "BT Connect Call..."); + bluetoothSocket.connect(); // Here the IOException will rise on BT + // protocol/handshake error. + + Log.d(BLUE, "## BT Connected ##"); + out = bluetoothSocket.getOutputStream(); + in = bluetoothSocket.getInputStream(); onConnectionOpened(); - } - - @SuppressLint("NewApi") - private BluetoothDevice findSerialBluetoothBoard() throws UnknownHostException { - Set pairedDevices = mBluetoothAdapter.getBondedDevices(); - // If there are paired devices - if (pairedDevices.size() > 0) { - // Loop through paired devices - for (BluetoothDevice device : pairedDevices) { - // Add the name and address to an array adapter to show in a ListView - Log.d(BLUE, device.getName() + " #" + device.getAddress() + "#"); - - final ParcelUuid[] deviceUuids = device.getUuids(); - if (deviceUuids != null && deviceUuids.length > 0) { - for (ParcelUuid id : device.getUuids()) { - // TODO maybe this will not work on newer devices - Log.d(BLUE, "id:" + id.toString()); - if (id.toString().equalsIgnoreCase(UUID_SPP_DEVICE)) { - Log.d(BLUE, - ">> Selected: " + device.getName() + " Using: " + id.toString()); - return device; - } - } - } - } - } - - throw new UnknownHostException("No Bluetooth Device found"); - } - - @Override - protected int readDataBlock(byte[] buffer) throws IOException { - return in.read(buffer); - - } - - @Override - protected void sendBuffer(byte[] buffer) throws IOException { - if (out != null) { - out.write(buffer); - } - } - - @Override - public int getConnectionType() { - return MavLinkConnectionTypes.MAVLINK_CONNECTION_BLUETOOTH; - } - - @Override - protected void closeConnection() throws IOException { - resetConnection(); - Log.d(BLUE, "## BT Closed ##"); - } - - private void resetConnection() throws IOException { - if (in != null) { - in.close(); - in = null; - } - - if (out != null) { - out.close(); - out = null; - } - - if (bluetoothSocket != null) { - bluetoothSocket.close(); - bluetoothSocket = null; - } - - } - - @Override - protected void loadPreferences() { - // TODO Auto-generated method stub - } + } + + @SuppressLint("NewApi") + private BluetoothDevice findSerialBluetoothBoard() throws UnknownHostException { + Set pairedDevices = mBluetoothAdapter.getBondedDevices(); + // If there are paired devices + if (pairedDevices.size() > 0) { + // Loop through paired devices + for (BluetoothDevice device : pairedDevices) { + // Add the name and address to an array adapter to show in a ListView + Log.d(BLUE, device.getName() + " #" + device.getAddress() + "#"); + + final ParcelUuid[] deviceUuids = device.getUuids(); + if (deviceUuids != null && deviceUuids.length > 0) { + for (ParcelUuid id : device.getUuids()) { + // TODO maybe this will not work on newer devices + Log.d(BLUE, "id:" + id.toString()); + if (id.toString().equalsIgnoreCase(UUID_SPP_DEVICE)) { + Log.d(BLUE, + ">> Selected: " + device.getName() + " Using: " + id.toString()); + return device; + } + } + } + } + } + + throw new UnknownHostException("No Bluetooth Device found"); + } + + @Override + protected int readDataBlock(byte[] buffer) throws IOException { + return in.read(buffer); + + } + + @Override + protected void sendBuffer(byte[] buffer) throws IOException { + if (out != null) { + out.write(buffer); + } + } + + @Override + public int getConnectionType() { + return MavLinkConnectionTypes.MAVLINK_CONNECTION_BLUETOOTH; + } + + @Override + protected void closeConnection() throws IOException { + resetConnection(); + Log.d(BLUE, "## BT Closed ##"); + } + + private void resetConnection() throws IOException { + if (in != null) { + in.close(); + in = null; + } + + if (out != null) { + out.close(); + out = null; + } + + if (bluetoothSocket != null) { + bluetoothSocket.close(); + bluetoothSocket = null; + } + + } + + @Override + protected void loadPreferences() { + // TODO Auto-generated method stub + } } diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java index 1255fd3b8a..745ca6eb2b 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java @@ -8,6 +8,7 @@ import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.ConnectionType; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import org.droidplanner.services.android.utils.connection.WifiConnectionHandler; @@ -43,8 +44,8 @@ protected void onConnectionOpened() { } @Override - protected void onConnectionFailed(String errMsg) { - SoloConnection.this.onConnectionFailed(errMsg); + protected void onConnectionStatus(LinkConnectionStatus connectionStatus) { + SoloConnection.this.onConnectionStatus(connectionStatus); } }; } @@ -52,16 +53,22 @@ protected void onConnectionFailed(String errMsg) { @Override protected void openConnection() throws IOException { if (TextUtils.isEmpty(soloLinkId)) { - throw new IOException("Invalid connection credentials!"); + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.INVALID_CREDENTIALS); + extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Invalid connection credentials!"); + onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + } else { + wifiHandler.start(); + checkScanResults(wifiHandler.getScanResults()); } - - wifiHandler.start(); - checkScanResults(wifiHandler.getScanResults()); } private void refreshWifiAps() { if (!wifiHandler.refreshWifiAPs()) { - onConnectionFailed("Unable to refresh wifi access points"); + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.SYSTEM_UNAVAILABLE); + extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Unable to refresh wifi access points"); + onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); } } @@ -100,8 +107,8 @@ public void onWifiConnected(String wifiSsid) { try { dataLink.openConnection(); } catch (IOException e) { + reportIOException(e); Timber.e(e, e.getMessage()); - onConnectionFailed(e.getMessage()); } } } @@ -109,12 +116,12 @@ public void onWifiConnected(String wifiSsid) { @Override public void onWifiConnecting() { - + onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.CONNECTING, null)); } @Override public void onWifiDisconnected() { - + onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.DISCONNECTED, null)); } @Override @@ -139,12 +146,19 @@ private void checkScanResults(List results) { if (targetResult != null) { //We're good to go try { - if (!wifiHandler.connectToWifi(targetResult, soloLinkPassword)) { - onConnectionFailed("Unable to connect to the target wifi " + soloLinkId); + int connectionResult = wifiHandler.connectToWifi(targetResult, soloLinkPassword); + if (connectionResult != 0) { + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, connectionResult); + extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Unable to connect to the target wifi " + soloLinkId); + onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); } } catch (IllegalArgumentException e) { Timber.e(e, e.getMessage()); - onConnectionFailed(e.getMessage()); + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.UNKNOWN); + extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, e.getMessage()); + onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); } } else { //Let's try again diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java index 0008a77d6e..4ea7289ac7 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java @@ -7,10 +7,12 @@ import android.content.IntentFilter; import android.hardware.usb.UsbDevice; import android.hardware.usb.UsbManager; +import android.os.Bundle; import android.util.Log; import com.hoho.android.usbserial.driver.UsbSerialDriver; import com.hoho.android.usbserial.driver.UsbSerialProber; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import java.io.IOException; import java.util.List; @@ -48,11 +50,17 @@ public void onReceive(Context context, Intent intent) { Log.e(TAG, e.getMessage(), e); } } else { - onUsbConnectionFailed("Unable to access usb device."); + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.LINK_UNAVAILABLE); + extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Unable to access usb device."); + onUsbConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); } } else { Log.d(TAG, "permission denied for device " + device); - onUsbConnectionFailed("USB Permission denied."); + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.PERMISSION_DENIED); + extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "USB Permission denied."); + onUsbConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); } } } @@ -62,7 +70,10 @@ public void onReceive(Context context, Intent intent) { @Override public void run() { Log.d(TAG, "Permission request timeout."); - onUsbConnectionFailed("Unable to get usb access."); + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.TIMEOUT); + extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Unable to get usb access."); + onUsbConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); removeWatchdog(); } diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbConnection.java index 58b6937ea0..08fef57bfd 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbConnection.java @@ -1,155 +1,158 @@ package org.droidplanner.services.android.communication.connection.usb; -import java.io.IOException; -import java.util.HashMap; -import java.util.Map.Entry; +import android.content.Context; +import android.hardware.usb.UsbDevice; +import android.hardware.usb.UsbManager; +import android.util.Log; + +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import org.droidplanner.services.android.communication.connection.AndroidMavLinkConnection; -import org.droidplanner.services.android.utils.AndroidLogger; import org.droidplanner.services.android.core.MAVLink.connection.MavLinkConnectionTypes; import org.droidplanner.services.android.core.model.Logger; +import org.droidplanner.services.android.utils.AndroidLogger; -import android.content.Context; -import android.hardware.usb.UsbDevice; -import android.hardware.usb.UsbManager; -import android.util.Log; +import java.io.IOException; +import java.util.HashMap; +import java.util.Map.Entry; public class UsbConnection extends AndroidMavLinkConnection { - private static final String TAG = UsbConnection.class.getSimpleName(); + private static final String TAG = UsbConnection.class.getSimpleName(); - private static final int FTDI_DEVICE_VENDOR_ID = 0x0403; + private static final int FTDI_DEVICE_VENDOR_ID = 0x0403; - protected final int mBaudRate; + protected final int mBaudRate; - private UsbConnectionImpl mUsbConnection; + private UsbConnectionImpl mUsbConnection; - public UsbConnection(Context parentContext, int baudRate) { - super(parentContext); + public UsbConnection(Context parentContext, int baudRate) { + super(parentContext); mBaudRate = baudRate; - } - - @Override - protected void closeConnection() throws IOException { - if (mUsbConnection != null) { - mUsbConnection.closeUsbConnection(); - } - } - - @Override - protected void loadPreferences() {} - - @Override - protected void openConnection() throws IOException { - if (mUsbConnection != null) { - try { - mUsbConnection.openUsbConnection(); - Log.d(TAG, "Reusing previous usb connection."); - return; - } catch (IOException e) { - Log.e(TAG, "Previous usb connection is not usable.", e); - mUsbConnection = null; - } - } - - if (isFTDIdevice(mContext)) { - final UsbConnectionImpl tmp = new UsbFTDIConnection(mContext, this, mBaudRate); - try { - tmp.openUsbConnection(); - - // If the call above is successful, 'mUsbConnection' will be set. - mUsbConnection = tmp; - Log.d(TAG, "Using FTDI usb connection."); - } catch (IOException e) { - Log.d(TAG, "Unable to open a ftdi usb connection. Falling back to the open " - + "usb-library.", e); - } - } - - // Fallback - if (mUsbConnection == null) { - final UsbConnectionImpl tmp = new UsbCDCConnection(mContext, this, mBaudRate); - - // If an error happens here, let it propagate up the call chain since this is the fallback. - tmp.openUsbConnection(); - mUsbConnection = tmp; - Log.d(TAG, "Using open-source usb connection."); - } - } - - private static boolean isFTDIdevice(Context context) { - UsbManager manager = (UsbManager) context.getSystemService(Context.USB_SERVICE); - final HashMap deviceList = manager.getDeviceList(); - if (deviceList == null || deviceList.isEmpty()) { - return false; - } - - for (Entry device : deviceList.entrySet()) { - if (device.getValue().getVendorId() == FTDI_DEVICE_VENDOR_ID) { - return true; - } - } - return false; - } - - @Override - protected int readDataBlock(byte[] buffer) throws IOException { - if (mUsbConnection == null) { - throw new IOException("Uninitialized usb connection."); - } - - return mUsbConnection.readDataBlock(buffer); - } - - @Override - protected void sendBuffer(byte[] buffer) throws IOException { - if (mUsbConnection == null) { - throw new IOException("Uninitialized usb connection."); - } - - mUsbConnection.sendBuffer(buffer); - } - - @Override - public int getConnectionType() { - return MavLinkConnectionTypes.MAVLINK_CONNECTION_USB; - } - - @Override - public String toString() { - if (mUsbConnection == null) { - return TAG; - } - - return mUsbConnection.toString(); - } - - static abstract class UsbConnectionImpl { - protected final int mBaudRate; - protected final Context mContext; + } + + @Override + protected void closeConnection() throws IOException { + if (mUsbConnection != null) { + mUsbConnection.closeUsbConnection(); + } + } + + @Override + protected void loadPreferences() { + } + + @Override + protected void openConnection() throws IOException { + if (mUsbConnection != null) { + try { + mUsbConnection.openUsbConnection(); + Log.d(TAG, "Reusing previous usb connection."); + return; + } catch (IOException e) { + Log.e(TAG, "Previous usb connection is not usable.", e); + mUsbConnection = null; + } + } + + if (isFTDIdevice(mContext)) { + final UsbConnectionImpl tmp = new UsbFTDIConnection(mContext, this, mBaudRate); + try { + tmp.openUsbConnection(); + + // If the call above is successful, 'mUsbConnection' will be set. + mUsbConnection = tmp; + Log.d(TAG, "Using FTDI usb connection."); + } catch (IOException e) { + Log.d(TAG, "Unable to open a ftdi usb connection. Falling back to the open " + + "usb-library.", e); + } + } + + // Fallback + if (mUsbConnection == null) { + final UsbConnectionImpl tmp = new UsbCDCConnection(mContext, this, mBaudRate); + + // If an error happens here, let it propagate up the call chain since this is the fallback. + tmp.openUsbConnection(); + mUsbConnection = tmp; + Log.d(TAG, "Using open-source usb connection."); + } + } + + private static boolean isFTDIdevice(Context context) { + UsbManager manager = (UsbManager) context.getSystemService(Context.USB_SERVICE); + final HashMap deviceList = manager.getDeviceList(); + if (deviceList == null || deviceList.isEmpty()) { + return false; + } + + for (Entry device : deviceList.entrySet()) { + if (device.getValue().getVendorId() == FTDI_DEVICE_VENDOR_ID) { + return true; + } + } + return false; + } + + @Override + protected int readDataBlock(byte[] buffer) throws IOException { + if (mUsbConnection == null) { + throw new IOException("Uninitialized usb connection."); + } + + return mUsbConnection.readDataBlock(buffer); + } + + @Override + protected void sendBuffer(byte[] buffer) throws IOException { + if (mUsbConnection == null) { + throw new IOException("Uninitialized usb connection."); + } + + mUsbConnection.sendBuffer(buffer); + } + + @Override + public int getConnectionType() { + return MavLinkConnectionTypes.MAVLINK_CONNECTION_USB; + } + + @Override + public String toString() { + if (mUsbConnection == null) { + return TAG; + } + + return mUsbConnection.toString(); + } + + static abstract class UsbConnectionImpl { + protected final int mBaudRate; + protected final Context mContext; private final UsbConnection parentConnection; - protected final Logger mLogger = AndroidLogger.getLogger(); + protected final Logger mLogger = AndroidLogger.getLogger(); - protected UsbConnectionImpl(Context context, UsbConnection parentConn, int baudRate) { - mContext = context; + protected UsbConnectionImpl(Context context, UsbConnection parentConn, int baudRate) { + mContext = context; this.parentConnection = parentConn; - mBaudRate = baudRate; - } + mBaudRate = baudRate; + } - protected void onUsbConnectionOpened(){ + protected void onUsbConnectionOpened() { parentConnection.onConnectionOpened(); } - protected void onUsbConnectionFailed(String errMsg){ - parentConnection.onConnectionFailed(errMsg); + protected void onUsbConnectionStatus(LinkConnectionStatus connectionStatus) { + parentConnection.onConnectionStatus(connectionStatus); } - protected abstract void closeUsbConnection() throws IOException; + protected abstract void closeUsbConnection() throws IOException; - protected abstract void openUsbConnection() throws IOException; + protected abstract void openUsbConnection() throws IOException; - protected abstract int readDataBlock(byte[] readData) throws IOException; + protected abstract int readDataBlock(byte[] readData) throws IOException; - protected abstract void sendBuffer(byte[] buffer); - } + protected abstract void sendBuffer(byte[] buffer); + } } diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/model/DataLink.java b/ServiceApp/src/org/droidplanner/services/android/communication/model/DataLink.java index a56f1a83e3..11afb5cf12 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/model/DataLink.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/model/DataLink.java @@ -1,5 +1,6 @@ package org.droidplanner.services.android.communication.model; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import com.o3dr.services.android.lib.model.ICommandListener; public class DataLink { @@ -17,14 +18,9 @@ public interface DataLinkProvider { } public interface DataLinkListener { - void notifyStartingConnection(); - - void notifyConnected(); - - void notifyDisconnected(); void notifyReceivedData(T packet); - void onStreamError(String errorMsg); + void onConnectionStatus(LinkConnectionStatus connectionStatus); } } diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java b/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java index 7b4ee8fe3f..58295089ac 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java @@ -9,6 +9,7 @@ import com.google.android.gms.analytics.HitBuilders; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.ConnectionType; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import com.o3dr.services.android.lib.model.ICommandListener; import org.droidplanner.services.android.communication.connection.AndroidMavLinkConnection; @@ -51,33 +52,30 @@ public class MAVLinkClient implements DataLink.DataLinkProvider private final MavLinkConnectionListener mConnectionListener = new MavLinkConnectionListener() { - @Override - public void onStartingConnection() { - listener.notifyStartingConnection(); - } - - @Override - public void onConnect(long connectionTime) { - startLoggingThread(connectionTime); - listener.notifyConnected(); - } - @Override public void onReceivePacket(final MAVLinkPacket packet) { listener.notifyReceivedData(packet); } @Override - public void onDisconnect(long disconnectTime) { - listener.notifyDisconnected(); - closeConnection(); - } + public void onConnectionStatus(final LinkConnectionStatus connectionStatus) { + switch (connectionStatus.getStatusCode()) { + case LinkConnectionStatus.DISCONNECTED: + closeConnection(); + break; - @Override - public void onComError(final String errMsg) { - if (errMsg != null) { - listener.onStreamError(errMsg); + case LinkConnectionStatus.CONNECTED: + Bundle extras = connectionStatus.getExtras(); + if (extras != null) { + long connectionTime = extras.getLong(LinkConnectionStatus.EXTRA_CONNECTION_TIME); + if (connectionTime != 0) { + startLoggingThread(connectionTime); + } + } + break; } + + listener.onConnectionStatus(connectionStatus); } }; @@ -225,7 +223,8 @@ public synchronized void closeConnection() { } stopLoggingThread(System.currentTimeMillis()); - listener.notifyDisconnected(); + + listener.onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.DISCONNECTED, null)); } @Override diff --git a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java index 0768faba4c..fc9041b828 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java @@ -1,15 +1,18 @@ package org.droidplanner.services.android.core.MAVLink.connection; +import android.os.Bundle; import android.support.v4.util.Pair; import com.MAVLink.MAVLinkPacket; import com.MAVLink.Parser; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import org.droidplanner.services.android.core.model.Logger; import java.io.BufferedOutputStream; import java.io.FileOutputStream; import java.io.IOException; +import java.net.BindException; import java.nio.ByteBuffer; import java.nio.ByteOrder; import java.util.Map; @@ -48,7 +51,7 @@ public abstract class MavLinkConnection { * Stores the list of log files to be written to. */ private final ConcurrentHashMap> loggingOutStreams = new - ConcurrentHashMap<>(); + ConcurrentHashMap<>(); /** * Queue the set of packets to send via the mavlink connection. A thread @@ -79,7 +82,8 @@ public void run() { } catch (IOException e) { // Ignore errors while shutting down if (mConnectionStatus.get() != MAVLINK_DISCONNECTED) { - reportComError(e.getMessage()); + reportIOException(e); + mLogger.logErr(TAG, e); } @@ -90,6 +94,14 @@ public void run() { } }; + private int getErrorCode(IOException e) { + if (e instanceof BindException) { + return LinkConnectionStatus.ADDRESS_IN_USE; + } else { + return LinkConnectionStatus.UNKNOWN; + } + } + /** * Manages the receiving and sending of messages. */ @@ -127,7 +139,7 @@ public void run() { } catch (IOException e) { // Ignore errors while shutting down if (mConnectionStatus.get() != MAVLINK_DISCONNECTED) { - reportComError(e.getMessage()); + reportIOException(e); mLogger.logErr(TAG, e); } } finally { @@ -173,7 +185,7 @@ public void run() { sendBuffer(buffer); queueToLog(buffer); } catch (IOException e) { - reportComError(e.getMessage()); + reportIOException(e); mLogger.logErr(TAG, e); } } @@ -204,7 +216,7 @@ public void run() { logBuffer.putLong(System.currentTimeMillis() * 1000); for (Map.Entry> entry : loggingOutStreams - .entrySet()) { + .entrySet()) { final Pair logInfo = entry.getValue(); final String loggingFilePath = logInfo.first; try { @@ -223,14 +235,16 @@ public void run() { } } catch (InterruptedException e) { final String errorMessage = e.getMessage(); - if (errorMessage != null) + if (errorMessage != null) { mLogger.logVerbose(TAG, errorMessage); + } } finally { for (Pair entry : loggingOutStreams.values()) { final String loggingFilePath = entry.first; try { - if (entry.second != null) + if (entry.second != null) { entry.second.close(); + } } catch (IOException e) { mLogger.logErr(TAG, "IO Exception while closing " + loggingFilePath, e); } @@ -267,10 +281,15 @@ protected void onConnectionOpened() { } } - protected void onConnectionFailed(String errMsg) { - mLogger.logInfo(TAG, "Unable to establish connection: " + errMsg); - reportComError(errMsg); - disconnect(); + protected void onConnectionStatus(LinkConnectionStatus connectionStatus) { + switch (connectionStatus.getStatusCode()) { + case LinkConnectionStatus.FAILED: + mLogger.logInfo(TAG, "Unable to establish connection: " + connectionStatus.getStatusCode()); + disconnect(); + break; + } + + reportConnectionStatus(connectionStatus); } /** @@ -297,10 +316,10 @@ public void disconnect() { } closeConnection(); - reportDisconnect(disconnectTime); + reportDisconnect(); } catch (IOException e) { mLogger.logErr(TAG, e); - reportComError(e.getMessage()); + reportIOException(e); } } @@ -316,8 +335,9 @@ public void sendMavPacket(MAVLinkPacket packet) { } private void queueToLog(MAVLinkPacket packet) { - if (packet != null) + if (packet != null) { queueToLog(packet.encodePacket()); + } } private void queueToLog(byte[] packetData) { @@ -329,16 +349,19 @@ private void queueToLog(byte[] packetData) { } public void addLoggingPath(String tag, String loggingPath) { - if (tag == null || tag.length() == 0 || loggingPath == null || loggingPath.length() == 0) + if (tag == null || tag.length() == 0 || loggingPath == null || loggingPath.length() == 0) { return; + } - if (!loggingOutStreams.contains(tag)) + if (!loggingOutStreams.contains(tag)) { loggingOutStreams.put(tag, Pair.create(loggingPath, null)); + } } public void removeLoggingPath(String tag) { - if (tag == null || tag.length() == 0) + if (tag == null || tag.length() == 0) { return; + } Pair logInfo = loggingOutStreams.remove(tag); if (logInfo != null) { @@ -363,7 +386,9 @@ public void addMavLinkConnectionListener(String tag, MavLinkConnectionListener l mListeners.put(tag, listener); if (getConnectionStatus() == MAVLINK_CONNECTED) { - listener.onConnect(mConnectionTime.get()); + Bundle extras = new Bundle(); + extras.putLong(LinkConnectionStatus.EXTRA_CONNECTION_TIME, mConnectionTime.get()); + reportConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.CONNECTED, extras)); } } @@ -425,21 +450,20 @@ protected Logger getLogger() { * Utility method to notify the mavlink listeners about communication * errors. * - * @param errMsg + * @param connectionStatus */ - protected void reportComError(String errMsg) { - if (mListeners.isEmpty()) + protected void reportConnectionStatus(LinkConnectionStatus connectionStatus) { + if (mListeners.isEmpty()) { return; + } for (MavLinkConnectionListener listener : mListeners.values()) { - listener.onComError(errMsg); + listener.onConnectionStatus(connectionStatus); } } protected void reportConnecting() { - for (MavLinkConnectionListener listener : mListeners.values()) { - listener.onStartingConnection(); - } + reportConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.CONNECTING, null)); } /** @@ -447,22 +471,17 @@ protected void reportConnecting() { * connection. */ protected void reportConnect(long connectionTime) { - for (MavLinkConnectionListener listener : mListeners.values()) { - listener.onConnect(connectionTime); - } + Bundle extras = new Bundle(); + extras.putLong(LinkConnectionStatus.EXTRA_CONNECTION_TIME, connectionTime); + reportConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.CONNECTED, extras)); } /** * Utility method to notify the mavlink listeners about a connection * disconnect. */ - protected void reportDisconnect(long disconnectTime) { - if (mListeners.isEmpty()) - return; - - for (MavLinkConnectionListener listener : mListeners.values()) { - listener.onDisconnect(disconnectTime); - } + protected void reportDisconnect() { + reportConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.DISCONNECTED, null)); } /** @@ -471,12 +490,19 @@ protected void reportDisconnect(long disconnectTime) { * @param packet received mavlink packet */ private void reportReceivedPacket(MAVLinkPacket packet) { - if (mListeners.isEmpty()) + if (mListeners.isEmpty()) { return; + } for (MavLinkConnectionListener listener : mListeners.values()) { listener.onReceivePacket(packet); } } + protected void reportIOException(IOException e) { + Bundle extras = new Bundle(); + extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, getErrorCode(e)); + extras.putSerializable(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, e.getMessage()); + reportConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + } } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnectionListener.java b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnectionListener.java index 26a1c21cf4..ccc90cbb3b 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnectionListener.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnectionListener.java @@ -1,41 +1,24 @@ package org.droidplanner.services.android.core.MAVLink.connection; import com.MAVLink.MAVLinkPacket; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; /** * Provides updates about the mavlink connection. */ public interface MavLinkConnectionListener { - /** - * Called when a connection is being established. + * Called when data is received via the mavlink connection. + * + * @param packet received data */ - public void onStartingConnection(); - - /** - * Called when the mavlink connection is established. - */ - public void onConnect(long connectionTime); - - /** - * Called when data is received via the mavlink connection. - * - * @param packet - * received data - */ - public void onReceivePacket(MAVLinkPacket packet); + void onReceivePacket(MAVLinkPacket packet); - /** - * Called when the mavlink connection is disconnected. - */ - public void onDisconnect(long disconnectionTime); - - /** - * Provides information about communication error. - * - * @param errMsg - * error information - */ - public void onComError(String errMsg); + /** + * Provides information about communication error. + * + * @param connectionStatus error information + */ + void onConnectionStatus(LinkConnectionStatus connectionStatus); } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/TcpConnection.java b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/TcpConnection.java index 7ba59c330b..c450d155a0 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/TcpConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/TcpConnection.java @@ -12,60 +12,61 @@ */ public abstract class TcpConnection extends MavLinkConnection { - private static final int CONNECTION_TIMEOUT = 20 * 1000; // 20 secs in ms + private static final int CONNECTION_TIMEOUT = 20 * 1000; // 20 secs in ms - private Socket socket; - private BufferedOutputStream mavOut; - private BufferedInputStream mavIn; + private Socket socket; + private BufferedOutputStream mavOut; + private BufferedInputStream mavIn; - private String serverIP; - private int serverPort; + private String serverIP; + private int serverPort; - @Override - public final void openConnection() throws IOException { - getTCPStream(); + @Override + public final void openConnection() throws IOException { + getTCPStream(); onConnectionOpened(); - } + } - @Override - public final int readDataBlock(byte[] buffer) throws IOException { - return mavIn.read(buffer); - } + @Override + public final int readDataBlock(byte[] buffer) throws IOException { + return mavIn.read(buffer); + } - @Override - public final void sendBuffer(byte[] buffer) throws IOException { - if (mavOut != null) { - mavOut.write(buffer); - mavOut.flush(); - } - } + @Override + public final void sendBuffer(byte[] buffer) throws IOException { + if (mavOut != null) { + mavOut.write(buffer); + mavOut.flush(); + } + } - @Override - public final void loadPreferences() { - serverIP = loadServerIP(); - serverPort = loadServerPort(); - } + @Override + public final void loadPreferences() { + serverIP = loadServerIP(); + serverPort = loadServerPort(); + } - protected abstract int loadServerPort(); + protected abstract int loadServerPort(); - protected abstract String loadServerIP(); + protected abstract String loadServerIP(); - @Override - public final void closeConnection() throws IOException { - if (socket != null) - socket.close(); - } + @Override + public final void closeConnection() throws IOException { + if (socket != null) { + socket.close(); + } + } - private void getTCPStream() throws IOException { - InetAddress serverAddr = InetAddress.getByName(serverIP); - socket = new Socket(); - socket.connect(new InetSocketAddress(serverAddr, serverPort), CONNECTION_TIMEOUT); - mavOut = new BufferedOutputStream((socket.getOutputStream())); - mavIn = new BufferedInputStream(socket.getInputStream()); - } + private void getTCPStream() throws IOException { + InetAddress serverAddr = InetAddress.getByName(serverIP); + socket = new Socket(); + socket.connect(new InetSocketAddress(serverAddr, serverPort), CONNECTION_TIMEOUT); + mavOut = new BufferedOutputStream((socket.getOutputStream())); + mavIn = new BufferedInputStream(socket.getInputStream()); + } - @Override - public final int getConnectionType() { - return MavLinkConnectionTypes.MAVLINK_CONNECTION_TCP; - } + @Override + public final int getConnectionType() { + return MavLinkConnectionTypes.MAVLINK_CONNECTION_TCP; + } } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/UdpConnection.java b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/UdpConnection.java index 5abfb3f34a..54cc67a39e 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/UdpConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/UdpConnection.java @@ -11,92 +11,97 @@ */ public abstract class UdpConnection extends MavLinkConnection { - private AtomicReference socketRef = new AtomicReference<>(); - private int serverPort; - - private int hostPort; - private InetAddress hostAdd; - private DatagramPacket sendPacket; - private DatagramPacket receivePacket; - - private void getUdpStream() throws IOException { - final DatagramSocket socket = new DatagramSocket(serverPort); - socket.setBroadcast(true); - socket.setReuseAddress(true); + private AtomicReference socketRef = new AtomicReference<>(); + private int serverPort; + + private int hostPort; + private InetAddress hostAdd; + private DatagramPacket sendPacket; + private DatagramPacket receivePacket; + + private void getUdpStream() throws IOException { + final DatagramSocket socket = new DatagramSocket(serverPort); + socket.setBroadcast(true); + socket.setReuseAddress(true); socketRef.set(socket); - } + } - @Override - public final void closeConnection() throws IOException { + @Override + public final void closeConnection() throws IOException { final DatagramSocket socket = socketRef.get(); - if (socket != null) - socket.close(); - } + if (socket != null) { + socket.close(); + } + } - @Override - public final void openConnection() throws IOException { - getUdpStream(); + @Override + public final void openConnection() throws IOException { + getUdpStream(); onConnectionOpened(); - } + } - @Override - public final void sendBuffer(byte[] buffer) throws IOException { + @Override + public final void sendBuffer(byte[] buffer) throws IOException { final DatagramSocket socket = socketRef.get(); - if(socket == null) + if (socket == null) { return; - - try { - if (hostAdd != null) { // We can't send to our sister until they - // have connected to us - if(sendPacket == null) - sendPacket = new DatagramPacket(buffer, buffer.length, hostAdd, hostPort); - else{ - sendPacket.setData(buffer, 0, buffer.length); - sendPacket.setAddress(hostAdd); - sendPacket.setPort(hostPort); - } - socket.send(sendPacket); - } - } catch (Exception e) { - e.printStackTrace(); - } - } + } + + try { + if (hostAdd != null) { // We can't send to our sister until they + // have connected to us + if (sendPacket == null) { + sendPacket = new DatagramPacket(buffer, buffer.length, hostAdd, hostPort); + } else { + sendPacket.setData(buffer, 0, buffer.length); + sendPacket.setAddress(hostAdd); + sendPacket.setPort(hostPort); + } + socket.send(sendPacket); + } + } catch (Exception e) { + e.printStackTrace(); + } + } public void sendBuffer(InetAddress targetAddr, int targetPort, byte[] buffer) throws IOException { final DatagramSocket socket = socketRef.get(); - if(socket == null || targetAddr == null || buffer == null) + if (socket == null || targetAddr == null || buffer == null) { return; + } DatagramPacket packet = new DatagramPacket(buffer, buffer.length, targetAddr, targetPort); socket.send(packet); } - @Override - public final int readDataBlock(byte[] readData) throws IOException { - final DatagramSocket socket = socketRef.get(); - if (socket == null) - return 0; - - if (receivePacket == null) - receivePacket = new DatagramPacket(readData, readData.length); - else - receivePacket.setData(readData); - - socket.receive(receivePacket); - hostAdd = receivePacket.getAddress(); - hostPort = receivePacket.getPort(); - return receivePacket.getLength(); - } - - @Override - public final void loadPreferences() { - serverPort = loadServerPort(); - } - - @Override - public final int getConnectionType() { - return MavLinkConnectionTypes.MAVLINK_CONNECTION_UDP; - } - - protected abstract int loadServerPort(); + @Override + public final int readDataBlock(byte[] readData) throws IOException { + final DatagramSocket socket = socketRef.get(); + if (socket == null) { + return 0; + } + + if (receivePacket == null) { + receivePacket = new DatagramPacket(readData, readData.length); + } else { + receivePacket.setData(readData); + } + + socket.receive(receivePacket); + hostAdd = receivePacket.getAddress(); + hostPort = receivePacket.getPort(); + return receivePacket.getLength(); + } + + @Override + public final void loadPreferences() { + serverPort = loadServerPort(); + } + + @Override + public final int getConnectionType() { + return MavLinkConnectionTypes.MAVLINK_CONNECTION_UDP; + } + + protected abstract int loadServerPort(); } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java index fd4f769526..50475021bd 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java @@ -11,6 +11,7 @@ import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.ConnectionType; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.property.DroneAttribute; import com.o3dr.services.android.lib.drone.property.Parameter; import com.o3dr.services.android.lib.model.ICommandListener; @@ -22,7 +23,6 @@ import org.droidplanner.services.android.core.drone.autopilot.apm.solo.ArduSolo; import org.droidplanner.services.android.core.drone.autopilot.apm.solo.SoloComp; import org.droidplanner.services.android.core.drone.manager.MavLinkDroneManager; -import org.droidplanner.services.android.exception.ConnectionException; import org.droidplanner.services.android.utils.CommonApiUtils; import org.droidplanner.services.android.utils.analytics.GAUtils; @@ -32,7 +32,7 @@ * Bridge between the communication channel, the drone instance(s), and the connected client(s). */ public class DroneManager implements DataLink.DataLinkListener, DroneInterfaces.OnDroneListener, - DroneInterfaces.OnParameterManagerListener, LogMessageListener, DroneInterfaces.AttributeEventListener { + DroneInterfaces.OnParameterManagerListener, LogMessageListener, DroneInterfaces.AttributeEventListener { private static final String TAG = DroneManager.class.getSimpleName(); @@ -48,8 +48,8 @@ public class DroneManager implements DataLink.DataLinkListen protected T drone; protected final ConnectionParameter connectionParameter; - public static DroneManager generateDroneManager(Context context, ConnectionParameter connParams, Handler handler){ - switch(connParams.getConnectionType()){ + public static DroneManager generateDroneManager(Context context, ConnectionParameter connParams, Handler handler) { + switch (connParams.getConnectionType()) { default: return new MavLinkDroneManager(context, connParams, handler); } @@ -62,8 +62,9 @@ protected DroneManager(Context context, ConnectionParameter connParams, Handler } private void destroyAutopilot() { - if (drone == null) + if (drone == null) { return; + } drone.destroy(); drone = null; @@ -79,9 +80,10 @@ public void destroy() { } - public synchronized void connect(String appId, DroneApi listener) throws ConnectionException { - if (listener == null || TextUtils.isEmpty(appId)) + public synchronized void connect(String appId, DroneApi listener) { + if (listener == null || TextUtils.isEmpty(appId)) { return; + } connectedApps.put(appId, listener); doConnect(appId, listener); @@ -94,11 +96,7 @@ protected void doConnect(String appId, DroneApi listener) { private void disconnect() { if (!connectedApps.isEmpty()) { for (DroneApi client : connectedApps.values()) { - try { - disconnect(client.getClientInfo()); - } catch (ConnectionException e) { - Log.e(TAG, e.getMessage(), e); - } + disconnect(client.getClientInfo()); } } } @@ -110,10 +108,10 @@ protected boolean isCompanionComputerEnabled() { final int connectionType = connectionParameter.getConnectionType(); return drone instanceof ArduSolo - || - (connectionType == ConnectionType.TYPE_UDP && SoloComp.isAvailable(context) && doAnyListenersSupportSoloLinkApi()) - || - connectionType == ConnectionType.TYPE_SOLO; + || + (connectionType == ConnectionType.TYPE_UDP && SoloComp.isAvailable(context) && doAnyListenersSupportSoloLinkApi()) + || + connectionType == ConnectionType.TYPE_SOLO; } @@ -121,10 +119,11 @@ public int getConnectedAppsCount() { return connectedApps.size(); } - public void disconnect(DroneApi.ClientInfo clientInfo) throws ConnectionException { + public void disconnect(DroneApi.ClientInfo clientInfo) { String appId = clientInfo.appId; - if (TextUtils.isEmpty(appId)) + if (TextUtils.isEmpty(appId)) { return; + } Log.d(TAG, "Disconnecting client " + appId); DroneApi listener = connectedApps.remove(appId); @@ -143,24 +142,6 @@ protected void doDisconnect(String appId, DroneApi listener) { } } - @Override - public void notifyStartingConnection() { - if (drone != null) - onDroneEvent(DroneInterfaces.DroneEventsType.CONNECTING, drone); - } - - @Override - public void notifyConnected() { - // Start a new ga analytics session. The new session will be tagged - // with the mavlink connection mechanism, as well as whether the user has an active droneshare account. - GAUtils.startNewSession(null); - } - - @Override - public void notifyDisconnected() { - notifyDroneEvent(DroneInterfaces.DroneEventsType.DISCONNECTED); - } - protected void notifyDroneEvent(DroneInterfaces.DroneEventsType event) { if (drone != null) { drone.notifyDroneEvent(event); @@ -173,12 +154,27 @@ public void notifyReceivedData(D data) { } @Override - public void onStreamError(String errorMsg) { - if (connectedApps.isEmpty()) + public void onConnectionStatus(LinkConnectionStatus connectionStatus) { + switch (connectionStatus.getStatusCode()) { + case LinkConnectionStatus.DISCONNECTED: + notifyDroneEvent(DroneInterfaces.DroneEventsType.DISCONNECTED); + break; + case LinkConnectionStatus.CONNECTED: + // Start a new ga analytics session. The new session will be tagged + // with the mavlink connection mechanism, as well as whether the user has an active droneshare account. + GAUtils.startNewSession(null); + break; + case LinkConnectionStatus.CONNECTING: + notifyDroneEvent(DroneInterfaces.DroneEventsType.CONNECTING); + break; + } + + if (connectedApps.isEmpty()) { return; + } for (DroneApi droneEventsListener : connectedApps.values()) { - droneEventsListener.onConnectionFailed(errorMsg); + droneEventsListener.onConnectionStatus(connectionStatus); } } @@ -246,8 +242,9 @@ protected void notifyDroneAttributeEvent(String attributeEvent, Bundle eventInfo * @param checkForSoloLinkApi */ private void notifyDroneAttributeEvent(String attributeEvent, Bundle eventInfo, boolean checkForSoloLinkApi) { - if (TextUtils.isEmpty(attributeEvent) || connectedApps.isEmpty()) + if (TextUtils.isEmpty(attributeEvent) || connectedApps.isEmpty()) { return; + } for (DroneApi listener : connectedApps.values()) { if (checkForSoloLinkApi && !supportSoloLinkApi(listener)) { @@ -267,12 +264,14 @@ private boolean supportSoloLinkApi(DroneApi listener) { * @return */ private boolean doAnyListenersSupportSoloLinkApi() { - if (connectedApps.isEmpty()) + if (connectedApps.isEmpty()) { return false; + } for (DroneApi listener : connectedApps.values()) { - if (supportSoloLinkApi(listener)) + if (supportSoloLinkApi(listener)) { return true; + } } return false; @@ -287,8 +286,9 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { break; } - if (connectedApps.isEmpty()) + if (connectedApps.isEmpty()) { return; + } for (DroneApi droneEventsListener : connectedApps.values()) { droneEventsListener.onDroneEvent(event, drone); @@ -297,8 +297,9 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { @Override public void onBeginReceivingParameters() { - if (connectedApps.isEmpty()) + if (connectedApps.isEmpty()) { return; + } for (DroneApi droneEventsListener : connectedApps.values()) { droneEventsListener.onBeginReceivingParameters(); @@ -307,8 +308,9 @@ public void onBeginReceivingParameters() { @Override public void onParameterReceived(Parameter parameter, int index, int count) { - if (connectedApps.isEmpty()) + if (connectedApps.isEmpty()) { return; + } for (DroneApi droneEventsListener : connectedApps.values()) { droneEventsListener.onParameterReceived(parameter, index, count); @@ -317,8 +319,9 @@ public void onParameterReceived(Parameter parameter, int index, int count) { @Override public void onEndReceivingParameters() { - if (connectedApps.isEmpty()) + if (connectedApps.isEmpty()) { return; + } for (DroneApi droneEventsListener : connectedApps.values()) { droneEventsListener.onEndReceivingParameters(); @@ -331,8 +334,9 @@ public ConnectionParameter getConnectionParameter() { @Override public void onMessageLogged(int logLevel, String message) { - if (connectedApps.isEmpty()) + if (connectedApps.isEmpty()) { return; + } for (DroneApi listener : connectedApps.values()) { listener.onMessageLogged(logLevel, message); diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java index 2dc661f0ca..6f5b198560 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java @@ -16,6 +16,7 @@ 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.connection.ConnectionParameter; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.property.DroneAttribute; import com.o3dr.services.android.lib.gcs.action.FollowMeActions; import com.o3dr.services.android.lib.gcs.follow.FollowType; @@ -195,17 +196,6 @@ protected void doDisconnect(String appId, DroneApi listener) { } } - @Override - public void notifyConnected() { - super.notifyConnected(); - this.gcsHeartbeat.setActive(true); - } - - @Override - public void notifyDisconnected() { - super.notifyDisconnected(); - this.gcsHeartbeat.setActive(false); - } private void handleCommandAck(msg_command_ack ack) { if (ack != null) { @@ -236,6 +226,21 @@ public void notifyReceivedData(MAVLinkPacket packet) { } } + @Override + public void onConnectionStatus(LinkConnectionStatus connectionStatus) { + super.onConnectionStatus(connectionStatus); + + switch (connectionStatus.getStatusCode()) { + case LinkConnectionStatus.DISCONNECTED: + this.gcsHeartbeat.setActive(false); + break; + + case LinkConnectionStatus.CONNECTED: + this.gcsHeartbeat.setActive(true); + break; + } + } + @Override public DroneAttribute getAttribute(DroneApi.ClientInfo clientInfo, String attributeType) { switch (attributeType) { diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java index 011143d1ca..c258382d0f 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java @@ -20,6 +20,8 @@ import android.text.TextUtils; import android.widget.Toast; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; + import java.util.List; import java.util.concurrent.atomic.AtomicReference; @@ -281,9 +283,9 @@ public List getScanResults() { return wifiMgr.getScanResults(); } - public boolean connectToWifi(String soloLinkId, String password) { + public int connectToWifi(String soloLinkId, String password) { if (TextUtils.isEmpty(soloLinkId)) - return false; + return LinkConnectionStatus.INVALID_CREDENTIALS; ScanResult targetScanResult = null; final List scanResults = wifiMgr.getScanResults(); @@ -296,15 +298,15 @@ public boolean connectToWifi(String soloLinkId, String password) { if (targetScanResult == null) { Timber.i("No matching scan result was found for id %s", soloLinkId); - return false; + return LinkConnectionStatus.LINK_UNAVAILABLE; } return connectToWifi(targetScanResult, password); } - public boolean connectToWifi(ScanResult scanResult, String password) { + public int connectToWifi(ScanResult scanResult, String password) { if (scanResult == null) - return false; + return LinkConnectionStatus.LINK_UNAVAILABLE; Timber.d("Connecting to wifi " + scanResult.SSID); @@ -314,10 +316,10 @@ public boolean connectToWifi(ScanResult scanResult, String password) { Timber.d("Already connected to " + scanResult.SSID); notifyWifiConnected(scanResult.SSID); - return true; + return 0; } else if (isOnNetwork(scanResult.SSID)) { setDefaultNetworkIfNecessary(scanResult.SSID); - return true; + return 0; } @@ -327,10 +329,10 @@ public boolean connectToWifi(ScanResult scanResult, String password) { if (wifiConfig == null) { Timber.d("Connecting to closed wifi network."); if (TextUtils.isEmpty(password)) - return false; + return LinkConnectionStatus.INVALID_CREDENTIALS; if (!connectToClosedWifi(scanResult, password)) - return false; + return LinkConnectionStatus.UNKNOWN; wifiMgr.saveConfiguration(); wifiConfig = getWifiConfigs(scanResult.SSID); @@ -338,9 +340,9 @@ public boolean connectToWifi(ScanResult scanResult, String password) { if (wifiConfig != null) { wifiMgr.enableNetwork(wifiConfig.networkId, true); - return true; + return 0; } - return false; + return LinkConnectionStatus.UNKNOWN; } private WifiConfiguration getWifiConfigs(String networkSSID) { diff --git a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java index 6858c9b284..3eb952283e 100644 --- a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java +++ b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java @@ -248,7 +248,7 @@ private void checkSoloState() { @Override public void onDroneConnectionFailed(ConnectionResult result) { - alertUser("Connection Failed:" + result.getErrorMessage()); + alertUser("Connection Failed:" + result.getExtras()); } @Override From 659f356027d84eb128ddb386e6e4bdc9a9d2d21e Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Tue, 9 Feb 2016 09:44:59 -0800 Subject: [PATCH 03/37] Updated test with new API. Reformatted other tests --- .../services/android/BasicTest.java | 15 +--- .../core/helpers/coordinates/Coord2DTest.java | 10 +-- .../helpers/coordinates/CoordBoundsTest.java | 80 +++++++++---------- .../android/core/helpers/units/AreaTest.java | 44 +++++----- .../waypoints/ChangeSpeedImplTest.java | 36 ++++----- .../waypoints/ConditionYawImplTest.java | 30 +++---- .../core/mission/waypoints/LandImplTest.java | 30 +++---- 7 files changed, 117 insertions(+), 128 deletions(-) diff --git a/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java b/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java index d9b0ae031f..54111dc41f 100644 --- a/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java +++ b/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java @@ -9,6 +9,7 @@ import com.MAVLink.common.msg_command_long; import com.MAVLink.enums.MAV_CMD; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; +import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import org.droidplanner.services.android.communication.model.DataLink; import org.droidplanner.services.android.core.MAVLink.MavLinkCommands; @@ -40,24 +41,12 @@ public class BasicTest { private final Handler dpHandler = new Handler(); private final DataLink.DataLinkListener inputStreamListener = new DataLink.DataLinkListener() { - @Override - public void notifyStartingConnection() { - } - - @Override - public void notifyConnected() { - } - - @Override - public void notifyDisconnected() { - } - @Override public void notifyReceivedData(Object packet) { } @Override - public void onStreamError(String errorMsg) { + public void onConnectionStatus(LinkConnectionStatus connectionStatus) { } }; diff --git a/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/coordinates/Coord2DTest.java b/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/coordinates/Coord2DTest.java index cd93cd2d9f..392967e3ab 100644 --- a/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/coordinates/Coord2DTest.java +++ b/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/coordinates/Coord2DTest.java @@ -6,10 +6,10 @@ public class Coord2DTest extends TestCase { - public void testConstructor() { - LatLong point = new LatLong(10, -5.6); - assertEquals(10.0, point.getLatitude()); - assertEquals(-5.6, point.getLongitude()); - } + public void testConstructor() { + LatLong point = new LatLong(10, -5.6); + assertEquals(10.0, point.getLatitude()); + assertEquals(-5.6, point.getLongitude()); + } } diff --git a/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/coordinates/CoordBoundsTest.java b/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/coordinates/CoordBoundsTest.java index 40fceba529..5cde695c93 100644 --- a/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/coordinates/CoordBoundsTest.java +++ b/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/coordinates/CoordBoundsTest.java @@ -2,49 +2,49 @@ import com.o3dr.services.android.lib.coordinate.LatLong; -import java.util.ArrayList; - import junit.framework.TestCase; +import java.util.ArrayList; + public class CoordBoundsTest extends TestCase { - private LatLong origin; - private LatLong point1; - private LatLong point2; - private LatLong point3; - private LatLong point4; - private ArrayList list; - - @Override - protected void setUp() throws Exception { - super.setUp(); - - origin = new LatLong(0, 0); - point1 = new LatLong(2.0, 2.0); - point2 = new LatLong(-1.0, 3.0); - point3 = new LatLong(0.0, -5.0); - point4 = new LatLong(-6.0, -3.0); - - list = new ArrayList(); - list.add(origin); - list.add(point1); - list.add(point2); - list.add(point3); - list.add(point4); - } - - public void testSinglePoint() { - assertEquals(0.0, new CoordBounds(origin).getDiag()); - assertEquals(0.0, new CoordBounds(point1).getDiag()); - } - - public void testList() { - CoordBounds bounds = new CoordBounds(list); - - assertEquals(2.0, bounds.ne_1quadrant.getLatitude()); - assertEquals(3.0, bounds.ne_1quadrant.getLongitude()); - assertEquals(-6.0, bounds.sw_3quadrant.getLatitude()); - assertEquals(-5.0, bounds.sw_3quadrant.getLongitude()); - } + private LatLong origin; + private LatLong point1; + private LatLong point2; + private LatLong point3; + private LatLong point4; + private ArrayList list; + + @Override + protected void setUp() throws Exception { + super.setUp(); + + origin = new LatLong(0, 0); + point1 = new LatLong(2.0, 2.0); + point2 = new LatLong(-1.0, 3.0); + point3 = new LatLong(0.0, -5.0); + point4 = new LatLong(-6.0, -3.0); + + list = new ArrayList(); + list.add(origin); + list.add(point1); + list.add(point2); + list.add(point3); + list.add(point4); + } + + public void testSinglePoint() { + assertEquals(0.0, new CoordBounds(origin).getDiag()); + assertEquals(0.0, new CoordBounds(point1).getDiag()); + } + + public void testList() { + CoordBounds bounds = new CoordBounds(list); + + assertEquals(2.0, bounds.ne_1quadrant.getLatitude()); + assertEquals(3.0, bounds.ne_1quadrant.getLongitude()); + assertEquals(-6.0, bounds.sw_3quadrant.getLatitude()); + assertEquals(-5.0, bounds.sw_3quadrant.getLongitude()); + } } diff --git a/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/units/AreaTest.java b/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/units/AreaTest.java index 3d6e5999fc..5476372415 100644 --- a/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/units/AreaTest.java +++ b/ServiceApp/test/java/org/droidplanner/services/android/core/helpers/units/AreaTest.java @@ -4,34 +4,34 @@ public class AreaTest extends TestCase { - private Area oneSqMeter; + private Area oneSqMeter; - @Override - protected void setUp() throws Exception { - super.setUp(); - oneSqMeter = new Area(1.0); - } + @Override + protected void setUp() throws Exception { + super.setUp(); + oneSqMeter = new Area(1.0); + } - public void testValueInSqMeters() { - assertEquals(1.0, oneSqMeter.valueInSqMeters()); - } + public void testValueInSqMeters() { + assertEquals(1.0, oneSqMeter.valueInSqMeters()); + } - public void testSet() { - oneSqMeter.set(2.3); - assertEquals(2.3, oneSqMeter.valueInSqMeters()); - } + public void testSet() { + oneSqMeter.set(2.3); + assertEquals(2.3, oneSqMeter.valueInSqMeters()); + } - public void testToString() { - assertEquals("1.0 m²", oneSqMeter.toString()); + public void testToString() { + assertEquals("1.0 m²", oneSqMeter.toString()); - oneSqMeter.set(1e6); - assertEquals("1.0 km²", oneSqMeter.toString()); + oneSqMeter.set(1e6); + assertEquals("1.0 km²", oneSqMeter.toString()); - oneSqMeter.set(1e-4); - assertEquals("1.00 cm²", oneSqMeter.toString()); + oneSqMeter.set(1e-4); + assertEquals("1.00 cm²", oneSqMeter.toString()); - oneSqMeter.set(1e-10); - assertEquals("1.0E-10 m²", oneSqMeter.toString()); - } + oneSqMeter.set(1e-10); + assertEquals("1.0E-10 m²", oneSqMeter.toString()); + } } diff --git a/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/ChangeSpeedImplTest.java b/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/ChangeSpeedImplTest.java index a57a0aaf16..b5b6fbe13c 100644 --- a/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/ChangeSpeedImplTest.java +++ b/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/ChangeSpeedImplTest.java @@ -1,34 +1,34 @@ package org.droidplanner.services.android.core.mission.waypoints; -import java.util.List; +import com.MAVLink.common.msg_mission_item; +import com.MAVLink.enums.MAV_CMD; import junit.framework.TestCase; import org.droidplanner.services.android.core.mission.Mission; import org.droidplanner.services.android.core.mission.commands.ChangeSpeedImpl; -import com.MAVLink.common.msg_mission_item; -import com.MAVLink.enums.MAV_CMD; +import java.util.List; public class ChangeSpeedImplTest extends TestCase { - public void testPackMissionItem() { - Mission mission = new Mission(null); - ChangeSpeedImpl item = new ChangeSpeedImpl(mission, 12.0); + public void testPackMissionItem() { + Mission mission = new Mission(null); + ChangeSpeedImpl item = new ChangeSpeedImpl(mission, 12.0); - List listOfMsg = item.packMissionItem(); - assertEquals(1, listOfMsg.size()); + List listOfMsg = item.packMissionItem(); + assertEquals(1, listOfMsg.size()); - msg_mission_item msg = listOfMsg.get(0); + msg_mission_item msg = listOfMsg.get(0); - assertEquals(MAV_CMD.MAV_CMD_DO_CHANGE_SPEED, msg.command); - assertEquals(0.0f, msg.x); - assertEquals(0.0f, msg.y); - assertEquals(0.0f, msg.z); - assertEquals(0.0f, msg.param1); - assertEquals(12.0f, msg.param2); - assertEquals(0.0f, msg.param3); - assertEquals(0.0f, msg.param3); - } + assertEquals(MAV_CMD.MAV_CMD_DO_CHANGE_SPEED, msg.command); + assertEquals(0.0f, msg.x); + assertEquals(0.0f, msg.y); + assertEquals(0.0f, msg.z); + assertEquals(0.0f, msg.param1); + assertEquals(12.0f, msg.param2); + assertEquals(0.0f, msg.param3); + assertEquals(0.0f, msg.param3); + } } diff --git a/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/ConditionYawImplTest.java b/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/ConditionYawImplTest.java index aa72398225..aa28ddc4ad 100644 --- a/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/ConditionYawImplTest.java +++ b/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/ConditionYawImplTest.java @@ -1,31 +1,31 @@ package org.droidplanner.services.android.core.mission.waypoints; -import java.util.List; +import com.MAVLink.common.msg_mission_item; +import com.MAVLink.enums.MAV_CMD; import junit.framework.TestCase; import org.droidplanner.services.android.core.mission.Mission; import org.droidplanner.services.android.core.mission.commands.ConditionYawImpl; -import com.MAVLink.common.msg_mission_item; -import com.MAVLink.enums.MAV_CMD; +import java.util.List; public class ConditionYawImplTest extends TestCase { - public void testPackMissionItem() { - Mission mission = new Mission(null); - ConditionYawImpl item = new ConditionYawImpl(mission, 12,false); + public void testPackMissionItem() { + Mission mission = new Mission(null); + ConditionYawImpl item = new ConditionYawImpl(mission, 12, false); - List listOfMsg = item.packMissionItem(); - assertEquals(1, listOfMsg.size()); + List listOfMsg = item.packMissionItem(); + assertEquals(1, listOfMsg.size()); - msg_mission_item msg = listOfMsg.get(0); + msg_mission_item msg = listOfMsg.get(0); - assertEquals(MAV_CMD.MAV_CMD_CONDITION_YAW, msg.command); - assertEquals(12.0f, msg.param1); - assertEquals(0.0f, msg.param2); - assertEquals(-1.0f, msg.param3); - assertEquals(0.0f, msg.param4); - } + assertEquals(MAV_CMD.MAV_CMD_CONDITION_YAW, msg.command); + assertEquals(12.0f, msg.param1); + assertEquals(0.0f, msg.param2); + assertEquals(-1.0f, msg.param3); + assertEquals(0.0f, msg.param4); + } } diff --git a/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/LandImplTest.java b/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/LandImplTest.java index 67815a54e0..8b062c8b4e 100644 --- a/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/LandImplTest.java +++ b/ServiceApp/test/java/org/droidplanner/services/android/core/mission/waypoints/LandImplTest.java @@ -1,30 +1,30 @@ package org.droidplanner.services.android.core.mission.waypoints; -import java.util.List; +import com.MAVLink.common.msg_mission_item; +import com.MAVLink.enums.MAV_CMD; import junit.framework.TestCase; import org.droidplanner.services.android.core.mission.Mission; -import com.MAVLink.common.msg_mission_item; -import com.MAVLink.enums.MAV_CMD; +import java.util.List; public class LandImplTest extends TestCase { - public void testPackMissionItem() { - Mission mission = new Mission(null); - LandImpl item = new LandImpl(mission); + public void testPackMissionItem() { + Mission mission = new Mission(null); + LandImpl item = new LandImpl(mission); - List listOfMsg = item.packMissionItem(); - assertEquals(1, listOfMsg.size()); + List listOfMsg = item.packMissionItem(); + assertEquals(1, listOfMsg.size()); - msg_mission_item msg = listOfMsg.get(0); + msg_mission_item msg = listOfMsg.get(0); - assertEquals(MAV_CMD.MAV_CMD_NAV_LAND, msg.command); - assertEquals(0.0f, msg.param1); - assertEquals(0.0f, msg.param2); - assertEquals(0.0f, msg.param3); - assertEquals(0.0f, msg.param3); - } + assertEquals(MAV_CMD.MAV_CMD_NAV_LAND, msg.command); + assertEquals(0.0f, msg.param1); + assertEquals(0.0f, msg.param2); + assertEquals(0.0f, msg.param3); + assertEquals(0.0f, msg.param3); + } } From c41ff1b407cf4f8fc76d3f52009608c4844090f9 Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Tue, 9 Feb 2016 10:33:33 -0800 Subject: [PATCH 04/37] Created helper method for failed connection status. Added javadocs --- .../connection/LinkConnectionStatus.java | 97 ++++++++++++++++--- .../services/android/api/DroneApi.java | 11 +-- .../connection/SoloConnection.java | 27 +++--- .../connection/usb/UsbCDCConnection.java | 22 ++--- .../MAVLink/connection/MavLinkConnection.java | 6 +- 5 files changed, 112 insertions(+), 51 deletions(-) diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java index 3ce5427072..929c8c951e 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java @@ -7,25 +7,47 @@ import android.support.annotation.StringDef; /** - * Conveys information if the connection attempt fails. + * Conveys information about the link connection state. + *

+ * This value is returned in the {@link com.o3dr.android.client.Drone#notifyAttributeUpdated} as the + * extra value {@link com.o3dr.services.android.lib.link.LinkEventExtra#EXTRA_CONNECTION_STATUS} + * when the attribute event is {@link com.o3dr.services.android.lib.link.LinkEvent#LINK_STATE_UPDATED} */ public final class LinkConnectionStatus implements Parcelable { + /** + * Key that is used to get the {@link FailureCode} from {@link #getExtras()} + * to determine what link connection error occurred. This will always be populated when {@link #FAILED} occurs. + */ + public static final String EXTRA_ERROR_CODE_KEY = "extra_error_code"; + + /** + * Key that is used to retrieve information from {@link #getExtras()} about why the link connection + * failure occurred. This value may be populated when {@link #FAILED} occurs, or can be null. + */ + public static final String EXTRA_ERROR_MSG_KEY = "extra_error_message"; + + /** + * Key that is used to retrieve the time a link connection occurred from {@link #getExtras()}. + * This is guaranteed when {@link #CONNECTED} occurs. + */ + public static final String EXTRA_CONNECTION_TIME = "extra_connection_time"; + @StringDef({ CONNECTED, CONNECTING, DISCONNECTED, FAILED }) - public @interface StatusCode{} + /** + * The possible status codes that notifies what state the link connection is in. + */ + public @interface StatusCode { + } public static final String CONNECTED = "CONNECTED"; public static final String CONNECTING = "CONNECTING"; public static final String DISCONNECTED = "DISCONNECTED"; public static final String FAILED = "FAILED"; - public static final String EXTRA_ERROR_CODE_KEY = "extra_error_code"; - public static final String EXTRA_ERROR_MSG_KEY = "extra_error_message"; - public static final String EXTRA_CONNECTION_TIME = "extra_connection_time"; - @IntDef({ SYSTEM_UNAVAILABLE, LINK_UNAVAILABLE, @@ -35,27 +57,65 @@ public final class LinkConnectionStatus implements Parcelable { ADDRESS_IN_USE, UNKNOWN }) - public @interface ErrorCode{} + /** + * The possible failure codes that can be retrieved from the {@link #getExtras()} using key + * {@link #EXTRA_ERROR_CODE_KEY}. A {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.FailureCode} + * is guaranteed when {@link #FAILED} occurs. + * + */ + public @interface FailureCode { + } + + /** + * The system does not allow the requested connection type. + */ public static final int SYSTEM_UNAVAILABLE = -1; + /** + * Requested device to connect to is not available. See {@link #EXTRA_ERROR_MSG_KEY} for more information. + */ public static final int LINK_UNAVAILABLE = -2; + /** + * Unable to access the requested connection type. + */ public static final int PERMISSION_DENIED = -3; + /** + * The provided credentials could not be authorized. + */ public static final int INVALID_CREDENTIALS = -4; + /** + * A timeout attempting to connect to device has occurred. + */ public static final int TIMEOUT = -5; + /** + * A {@link java.net.BindException} occurred, determining that the requested address is in use. + */ public static final int ADDRESS_IN_USE = -6; + /** + * All errors that are not one of the listed {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.FailureCode}s. + * This is usually due to a device system failure. + */ public static final int UNKNOWN = -7; - private final @StatusCode String mStatusCode; + @StatusCode + private final String mStatusCode; private final Bundle mExtras; - public LinkConnectionStatus(@StatusCode String errorCode, Bundle extras) { - this.mStatusCode = errorCode; + public LinkConnectionStatus(@StatusCode String statusCode, Bundle extras) { + this.mStatusCode = statusCode; this.mExtras = extras; } - public @StatusCode String getStatusCode() { + /** + * @return Returns the status of the link connection. This value is one of {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.StatusCode} + */ + @StatusCode + public String getStatusCode() { return mStatusCode; } + /** + * @return Returns a {@link Bundle} with additional information about the link connection. + */ public Bundle getExtras() { return mExtras; } @@ -120,4 +180,19 @@ public String toString() { ", mExtras=" + mExtras + '}'; } + + /** + * Helper method to generate the generic {@link #FAILED} {@link LinkConnectionStatus} + * @param failureCode Of type {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.FailureCode} + * @param errMsg A message that gives more information to the client about the error. This can be null. + * + * @return Returns a {@link LinkConnectionStatus} with statusCode {@link #FAILED} + */ + public static LinkConnectionStatus newFailedConnectionStatus(@FailureCode int failureCode, String errMsg) { + Bundle extras = new Bundle(); + extras.putInt(EXTRA_ERROR_CODE_KEY, failureCode); + extras.putString(EXTRA_ERROR_MSG_KEY, errMsg); + + return new LinkConnectionStatus(FAILED, extras); + } } diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index 1c80207dd1..bbda0b60b4 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -201,10 +201,9 @@ public void connect(ConnectionParameter connParams) { this.connectionParams = checkConnectionParameter(connParams); this.droneMgr = service.connectDroneManager(this.connectionParams, ownerId, this); } catch (ConnectionException e) { - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.INVALID_CREDENTIALS); - extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, e.getMessage()); - notifyConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + LinkConnectionStatus connectionStatus = LinkConnectionStatus + .newFailedConnectionStatus(LinkConnectionStatus.INVALID_CREDENTIALS, e.getMessage()); + onConnectionStatus(connectionStatus); disconnect(); } } @@ -547,9 +546,7 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { case CONNECTION_FAILED: disconnect(); - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.UNKNOWN); - onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + onConnectionStatus(LinkConnectionStatus.newFailedConnectionStatus(LinkConnectionStatus.UNKNOWN, null)); break; case HEARTBEAT_FIRST: diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java index 745ca6eb2b..0372539a33 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java @@ -53,10 +53,9 @@ protected void onConnectionStatus(LinkConnectionStatus connectionStatus) { @Override protected void openConnection() throws IOException { if (TextUtils.isEmpty(soloLinkId)) { - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.INVALID_CREDENTIALS); - extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Invalid connection credentials!"); - onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + LinkConnectionStatus connectionStatus = LinkConnectionStatus + .newFailedConnectionStatus(LinkConnectionStatus.INVALID_CREDENTIALS, "Invalid connection credentials!"); + onConnectionStatus(connectionStatus); } else { wifiHandler.start(); checkScanResults(wifiHandler.getScanResults()); @@ -65,10 +64,9 @@ protected void openConnection() throws IOException { private void refreshWifiAps() { if (!wifiHandler.refreshWifiAPs()) { - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.SYSTEM_UNAVAILABLE); - extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Unable to refresh wifi access points"); - onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + LinkConnectionStatus connectionStatus = LinkConnectionStatus + .newFailedConnectionStatus(LinkConnectionStatus.SYSTEM_UNAVAILABLE, "Unable to refresh wifi access points"); + onConnectionStatus(connectionStatus); } } @@ -148,17 +146,14 @@ private void checkScanResults(List results) { try { int connectionResult = wifiHandler.connectToWifi(targetResult, soloLinkPassword); if (connectionResult != 0) { - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, connectionResult); - extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Unable to connect to the target wifi " + soloLinkId); - onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + LinkConnectionStatus connectionStatus = LinkConnectionStatus + .newFailedConnectionStatus(connectionResult, "Unable to connect to the target wifi " + soloLinkId); + onConnectionStatus(connectionStatus); } } catch (IllegalArgumentException e) { Timber.e(e, e.getMessage()); - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.UNKNOWN); - extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, e.getMessage()); - onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + LinkConnectionStatus connectionStatus = LinkConnectionStatus.newFailedConnectionStatus(LinkConnectionStatus.UNKNOWN, e.getMessage()); + onConnectionStatus(connectionStatus); } } else { //Let's try again diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java index 4ea7289ac7..b9de771c78 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java @@ -7,7 +7,6 @@ import android.content.IntentFilter; import android.hardware.usb.UsbDevice; import android.hardware.usb.UsbManager; -import android.os.Bundle; import android.util.Log; import com.hoho.android.usbserial.driver.UsbSerialDriver; @@ -50,17 +49,15 @@ public void onReceive(Context context, Intent intent) { Log.e(TAG, e.getMessage(), e); } } else { - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.LINK_UNAVAILABLE); - extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Unable to access usb device."); - onUsbConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + LinkConnectionStatus connectionStatus = LinkConnectionStatus + .newFailedConnectionStatus(LinkConnectionStatus.LINK_UNAVAILABLE, "Unable to access usb device."); + onUsbConnectionStatus(connectionStatus); } } else { Log.d(TAG, "permission denied for device " + device); - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.PERMISSION_DENIED); - extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "USB Permission denied."); - onUsbConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + LinkConnectionStatus connectionStatus = LinkConnectionStatus + .newFailedConnectionStatus(LinkConnectionStatus.PERMISSION_DENIED, "USB Permission denied."); + onUsbConnectionStatus(connectionStatus); } } } @@ -70,10 +67,9 @@ public void onReceive(Context context, Intent intent) { @Override public void run() { Log.d(TAG, "Permission request timeout."); - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, LinkConnectionStatus.TIMEOUT); - extras.putString(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, "Unable to get usb access."); - onUsbConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + LinkConnectionStatus connectionStatus = LinkConnectionStatus + .newFailedConnectionStatus(LinkConnectionStatus.TIMEOUT, "Unable to get usb access."); + onUsbConnectionStatus(connectionStatus); removeWatchdog(); } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java index fc9041b828..7df85bac58 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java @@ -94,6 +94,7 @@ public void run() { } }; + @LinkConnectionStatus.FailureCode private int getErrorCode(IOException e) { if (e instanceof BindException) { return LinkConnectionStatus.ADDRESS_IN_USE; @@ -500,9 +501,6 @@ private void reportReceivedPacket(MAVLinkPacket packet) { } protected void reportIOException(IOException e) { - Bundle extras = new Bundle(); - extras.putInt(LinkConnectionStatus.EXTRA_ERROR_CODE_KEY, getErrorCode(e)); - extras.putSerializable(LinkConnectionStatus.EXTRA_ERROR_MSG_KEY, e.getMessage()); - reportConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.FAILED, extras)); + reportConnectionStatus(LinkConnectionStatus.newFailedConnectionStatus(getErrorCode(e), e.getMessage())); } } From 48fae6a1f620df3d2bd709c98834d5a942470f2c Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Wed, 10 Feb 2016 10:33:13 -0800 Subject: [PATCH 05/37] Updated per comments --- .../java/com/o3dr/android/client/Drone.java | 28 +++++++++---------- .../client/interfaces/DroneListener.java | 4 ++- .../drone/connection/ConnectionResult.java | 3 +- .../connection/LinkConnectionStatus.java | 19 +++++++------ .../android/lib/gcs/link/LinkEvent.java | 20 +++++++++++++ .../android/lib/gcs/link/LinkEventExtra.java | 21 ++++++++++++++ .../services/android/lib/link/LinkEvent.java | 15 ---------- .../android/lib/link/LinkEventExtra.java | 13 --------- .../services/android/api/DroneApi.java | 18 ++++++------ 9 files changed, 77 insertions(+), 64 deletions(-) create mode 100644 ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEvent.java create mode 100644 ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEventExtra.java delete mode 100644 ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEvent.java delete mode 100644 ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEventExtra.java 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 77b807305c..66129fddb7 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/Drone.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/Drone.java @@ -21,8 +21,6 @@ import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.drone.attribute.AttributeEvent; import com.o3dr.services.android.lib.drone.attribute.AttributeType; -import com.o3dr.services.android.lib.link.LinkEvent; -import com.o3dr.services.android.lib.link.LinkEventExtra; import com.o3dr.services.android.lib.drone.calibration.magnetometer.MagnetometerCalibrationStatus; import com.o3dr.services.android.lib.drone.companion.solo.SoloAttributes; import com.o3dr.services.android.lib.drone.companion.solo.SoloEventExtras; @@ -48,6 +46,8 @@ import com.o3dr.services.android.lib.gcs.follow.FollowState; import com.o3dr.services.android.lib.gcs.follow.FollowType; import com.o3dr.services.android.lib.gcs.returnToMe.ReturnToMeState; +import com.o3dr.services.android.lib.gcs.link.LinkEvent; +import com.o3dr.services.android.lib.gcs.link.LinkEventExtra; import com.o3dr.services.android.lib.mavlink.MavlinkMessageWrapper; import com.o3dr.services.android.lib.model.AbstractCommandListener; import com.o3dr.services.android.lib.model.IDroneApi; @@ -772,7 +772,7 @@ public void onRetrievalFailed() { final Bundle eventInfo = new Bundle(1); boolean isEUCompliant = !TxPowerComplianceCountries.getDefaultCountry().name().equals(compliantCountry); eventInfo.putBoolean(SoloEventExtras.EXTRA_SOLO_EU_TX_POWER_COMPLIANT, isEUCompliant); - sendAttributeEventToListener(SoloEvents.SOLO_EU_TX_POWER_COMPLIANCE_UPDATED, eventInfo); + sendDroneEventToListeners(SoloEvents.SOLO_EU_TX_POWER_COMPLIANCE_UPDATED, eventInfo); break; case LinkEvent.LINK_STATE_UPDATED: @@ -780,10 +780,10 @@ public void onRetrievalFailed() { return; } - sendAttributeEventToListener(attributeEvent, extras); + sendDroneEventToListeners(attributeEvent, extras); } - private void sendAttributeEventToListener(final String attributeEvent, final Bundle extras) { + private void sendDroneEventToListeners(final String attributeEvent, final Bundle extras) { if (droneListeners.isEmpty()) { return; } @@ -807,17 +807,15 @@ private void sendLinkEventToListener(final Bundle extras) { return; } - handler.post(new Runnable() { - @Override - public void run() { - LinkConnectionStatus status = null; - if (extras != null) { - status = extras.getParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS); + if (extras != null) { + handler.post(new Runnable() { + @Override + public void run() { + LinkConnectionStatus status = extras.getParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS); + linkListener.onLinkStateUpdated(status); } - - linkListener.onLinkStateUpdated(status); - } - }); + }); + } } void notifyDroneServiceInterrupted(final String errorMsg) { diff --git a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java index f312c1676d..32e0657b60 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java @@ -10,7 +10,9 @@ public interface DroneListener { /** - * @deprecated + * @deprecated Use {@link #onDroneEvent(String, Bundle)} with event + * {@link com.o3dr.services.android.lib.gcs.link.LinkEvent#LINK_STATE_UPDATED} instead. + * * @param result */ void onDroneConnectionFailed(ConnectionResult result); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java index 770fb250c7..b04e904e6b 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java @@ -4,7 +4,8 @@ import android.os.Parcelable; /** - * @deprecated + * @deprecated Use {@link LinkConnectionStatus} instead. + * * Conveys information if the connection attempt fails. */ public final class ConnectionResult implements Parcelable { diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java index 929c8c951e..9504f56ca2 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java @@ -4,27 +4,28 @@ import android.os.Parcel; import android.os.Parcelable; import android.support.annotation.IntDef; +import android.support.annotation.Nullable; import android.support.annotation.StringDef; /** * Conveys information about the link connection state. *

* This value is returned in the {@link com.o3dr.android.client.Drone#notifyAttributeUpdated} as the - * extra value {@link com.o3dr.services.android.lib.link.LinkEventExtra#EXTRA_CONNECTION_STATUS} - * when the attribute event is {@link com.o3dr.services.android.lib.link.LinkEvent#LINK_STATE_UPDATED} + * extra value {@link com.o3dr.services.android.lib.gcs.link.LinkEventExtra#EXTRA_CONNECTION_STATUS} + * when the attribute event is {@link com.o3dr.services.android.lib.gcs.link.LinkEvent#LINK_STATE_UPDATED} */ public final class LinkConnectionStatus implements Parcelable { /** * Key that is used to get the {@link FailureCode} from {@link #getExtras()} * to determine what link connection error occurred. This will always be populated when {@link #FAILED} occurs. */ - public static final String EXTRA_ERROR_CODE_KEY = "extra_error_code"; + public static final String EXTRA_ERROR_CODE = "extra_error_code"; /** * Key that is used to retrieve information from {@link #getExtras()} about why the link connection * failure occurred. This value may be populated when {@link #FAILED} occurs, or can be null. */ - public static final String EXTRA_ERROR_MSG_KEY = "extra_error_message"; + public static final String EXTRA_ERROR_MSG = "extra_error_message"; /** * Key that is used to retrieve the time a link connection occurred from {@link #getExtras()}. @@ -59,7 +60,7 @@ public final class LinkConnectionStatus implements Parcelable { }) /** * The possible failure codes that can be retrieved from the {@link #getExtras()} using key - * {@link #EXTRA_ERROR_CODE_KEY}. A {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.FailureCode} + * {@link #EXTRA_ERROR_CODE}. A {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.FailureCode} * is guaranteed when {@link #FAILED} occurs. * */ @@ -71,7 +72,7 @@ public final class LinkConnectionStatus implements Parcelable { */ public static final int SYSTEM_UNAVAILABLE = -1; /** - * Requested device to connect to is not available. See {@link #EXTRA_ERROR_MSG_KEY} for more information. + * Requested device to connect to is not available. See {@link #EXTRA_ERROR_MSG} for more information. */ public static final int LINK_UNAVAILABLE = -2; /** @@ -188,10 +189,10 @@ public String toString() { * * @return Returns a {@link LinkConnectionStatus} with statusCode {@link #FAILED} */ - public static LinkConnectionStatus newFailedConnectionStatus(@FailureCode int failureCode, String errMsg) { + public static LinkConnectionStatus newFailedConnectionStatus(@FailureCode int failureCode, @Nullable String errMsg) { Bundle extras = new Bundle(); - extras.putInt(EXTRA_ERROR_CODE_KEY, failureCode); - extras.putString(EXTRA_ERROR_MSG_KEY, errMsg); + extras.putInt(EXTRA_ERROR_CODE, failureCode); + extras.putString(EXTRA_ERROR_MSG, errMsg); return new LinkConnectionStatus(FAILED, extras); } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEvent.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEvent.java new file mode 100644 index 0000000000..2758155a1e --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEvent.java @@ -0,0 +1,20 @@ +package com.o3dr.services.android.lib.gcs.link; + +/** + * Stores all possible link events. + */ +public class LinkEvent { + + private LinkEvent() { + } + + private static final String PACKAGE_NAME = "com.o3dr.services.android.lib.gcs.link.event"; + + /** + * Notifies what the link connection status currently is. + * + * @see {@link LinkEventExtra#EXTRA_CONNECTION_STATUS} + */ + public static final String LINK_STATE_UPDATED = PACKAGE_NAME + ".LINK_STATE_UPDATED"; + +} diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEventExtra.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEventExtra.java new file mode 100644 index 0000000000..ea26378ab6 --- /dev/null +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEventExtra.java @@ -0,0 +1,21 @@ +package com.o3dr.services.android.lib.gcs.link; + +/** + * Holds handles used to retrieve additional information broadcast along a link event. + * + * @see {@link LinkEvent} + */ +public class LinkEventExtra { + private LinkEventExtra() { + } + + private static final String PACKAGE_NAME = "com.o3dr.services.android.lib.gcs.link.event.extra"; + + /** + * Used to access the link connection status. + * + * @see {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus} + * @see {@link com.o3dr.services.android.lib.gcs.link.LinkEvent#LINK_STATE_UPDATED} + */ + public static final String EXTRA_CONNECTION_STATUS = PACKAGE_NAME + ".CONNECTION_STATUS"; +} diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEvent.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEvent.java deleted file mode 100644 index fdd4927ce1..0000000000 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEvent.java +++ /dev/null @@ -1,15 +0,0 @@ -package com.o3dr.services.android.lib.link; - -/** - * Created by chavi on 2/5/16. - */ -public class LinkEvent { - - private LinkEvent() { - } - - private static final String PACKAGE_NAME = "com.o3dr.services.android.lib.link.event"; - - public static final String LINK_STATE_UPDATED = PACKAGE_NAME + ".LINK_STATE_UPDATED"; - -} diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEventExtra.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEventExtra.java deleted file mode 100644 index f625110f97..0000000000 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/link/LinkEventExtra.java +++ /dev/null @@ -1,13 +0,0 @@ -package com.o3dr.services.android.lib.link; - -/** - * Created by chavi on 2/5/16. - */ -public class LinkEventExtra { - private LinkEventExtra() { - } - - private static final String PACKAGE_NAME = "com.o3dr.services.android.lib.link.event.extra"; - - public static final String EXTRA_CONNECTION_STATUS = PACKAGE_NAME + ".CONNECTION_STATUS"; -} diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index bbda0b60b4..af707a0102 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -18,9 +18,9 @@ import com.o3dr.services.android.lib.drone.attribute.AttributeEvent; import com.o3dr.services.android.lib.drone.attribute.AttributeEventExtra; import com.o3dr.services.android.lib.drone.attribute.AttributeType; -import com.o3dr.services.android.lib.link.LinkEvent; +import com.o3dr.services.android.lib.gcs.link.LinkEvent; import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; -import com.o3dr.services.android.lib.link.LinkEventExtra; +import com.o3dr.services.android.lib.gcs.link.LinkEventExtra; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.mission.Mission; @@ -373,12 +373,6 @@ private void notifyAttributeUpdate(String attributeEvent, Bundle extrasBundle) { } } - private void notifyConnectionStatus(LinkConnectionStatus connectionStatus) { - Bundle extras = new Bundle(); - extras.putParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS, connectionStatus); - notifyAttributeUpdate(LinkEvent.LINK_STATE_UPDATED, extras); - } - public void onReceivedMavLinkMessage(MAVLinkMessage msg) { if (mavlinkObserversList.isEmpty()) { return; @@ -546,7 +540,7 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { case CONNECTION_FAILED: disconnect(); - onConnectionStatus(LinkConnectionStatus.newFailedConnectionStatus(LinkConnectionStatus.UNKNOWN, null)); + droneEvent = AttributeEvent.HEARTBEAT_TIMEOUT; break; case HEARTBEAT_FIRST: @@ -657,7 +651,11 @@ public void onEndReceivingParameters() { } public void onConnectionStatus(LinkConnectionStatus connectionStatus) { - notifyConnectionStatus(connectionStatus); + if (connectionStatus != null) { + Bundle extras = new Bundle(); + extras.putParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS, connectionStatus); + notifyAttributeUpdate(LinkEvent.LINK_STATE_UPDATED, extras); + } } @Override From 4f0cd3f6aa6a1e767b3129d1fa047181ae0c87d2 Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Wed, 10 Feb 2016 12:38:25 -0800 Subject: [PATCH 06/37] Ensure backwards compatibility for onConnectionFailed --- .../java/com/o3dr/android/client/Drone.java | 2 +- .../client/interfaces/LinkListener.java | 2 +- .../drone/connection/ConnectionResult.java | 2 ++ .../link}/LinkConnectionStatus.java | 10 +++--- .../android/lib/gcs/link/LinkEventExtra.java | 2 +- .../services/android/api/DroneApi.java | 33 +++++++++++++++++-- .../connection/SoloConnection.java | 2 +- .../connection/usb/UsbCDCConnection.java | 2 +- .../connection/usb/UsbConnection.java | 2 +- .../android/communication/model/DataLink.java | 2 +- .../communication/service/MAVLinkClient.java | 2 +- .../MAVLink/connection/MavLinkConnection.java | 2 +- .../connection/MavLinkConnectionListener.java | 2 +- .../android/core/drone/DroneManager.java | 2 +- .../drone/manager/MavLinkDroneManager.java | 2 +- .../connection/WifiConnectionHandler.java | 2 +- .../services/android/BasicTest.java | 2 +- 17 files changed, 51 insertions(+), 22 deletions(-) rename ClientLib/src/main/java/com/o3dr/services/android/lib/{drone/connection => gcs/link}/LinkConnectionStatus.java (92%) 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 66129fddb7..75bc715703 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/Drone.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/Drone.java @@ -27,7 +27,7 @@ import com.o3dr.services.android.lib.drone.companion.solo.SoloEvents; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.ConnectionResult; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.mission.Mission; import com.o3dr.services.android.lib.drone.mission.item.MissionItem; import com.o3dr.services.android.lib.drone.property.Altitude; diff --git a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java index 2d46119350..d084d74a12 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java @@ -1,6 +1,6 @@ package com.o3dr.android.client.interfaces; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; /** * Created by chavi on 2/8/16. diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java index b04e904e6b..c4d4a652fa 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionResult.java @@ -3,6 +3,8 @@ import android.os.Parcel; import android.os.Parcelable; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; + /** * @deprecated Use {@link LinkConnectionStatus} instead. * diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkConnectionStatus.java similarity index 92% rename from ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java rename to ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkConnectionStatus.java index 9504f56ca2..e9f1c95980 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/LinkConnectionStatus.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkConnectionStatus.java @@ -1,4 +1,4 @@ -package com.o3dr.services.android.lib.drone.connection; +package com.o3dr.services.android.lib.gcs.link; import android.os.Bundle; import android.os.Parcel; @@ -60,7 +60,7 @@ public final class LinkConnectionStatus implements Parcelable { }) /** * The possible failure codes that can be retrieved from the {@link #getExtras()} using key - * {@link #EXTRA_ERROR_CODE}. A {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.FailureCode} + * {@link #EXTRA_ERROR_CODE}. A {@link LinkConnectionStatus.FailureCode} * is guaranteed when {@link #FAILED} occurs. * */ @@ -92,7 +92,7 @@ public final class LinkConnectionStatus implements Parcelable { */ public static final int ADDRESS_IN_USE = -6; /** - * All errors that are not one of the listed {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.FailureCode}s. + * All errors that are not one of the listed {@link LinkConnectionStatus.FailureCode}s. * This is usually due to a device system failure. */ public static final int UNKNOWN = -7; @@ -107,7 +107,7 @@ public LinkConnectionStatus(@StatusCode String statusCode, Bundle extras) { } /** - * @return Returns the status of the link connection. This value is one of {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.StatusCode} + * @return Returns the status of the link connection. This value is one of {@link LinkConnectionStatus.StatusCode} */ @StatusCode public String getStatusCode() { @@ -184,7 +184,7 @@ public String toString() { /** * Helper method to generate the generic {@link #FAILED} {@link LinkConnectionStatus} - * @param failureCode Of type {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus.FailureCode} + * @param failureCode Of type {@link LinkConnectionStatus.FailureCode} * @param errMsg A message that gives more information to the client about the error. This can be null. * * @return Returns a {@link LinkConnectionStatus} with statusCode {@link #FAILED} diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEventExtra.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEventExtra.java index ea26378ab6..03559f95a1 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEventExtra.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/gcs/link/LinkEventExtra.java @@ -14,7 +14,7 @@ private LinkEventExtra() { /** * Used to access the link connection status. * - * @see {@link com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus} + * @see {@link LinkConnectionStatus} * @see {@link com.o3dr.services.android.lib.gcs.link.LinkEvent#LINK_STATE_UPDATED} */ public static final String EXTRA_CONNECTION_STATUS = PACKAGE_NAME + ".CONNECTION_STATUS"; diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index af707a0102..93a817e783 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -18,11 +18,10 @@ import com.o3dr.services.android.lib.drone.attribute.AttributeEvent; import com.o3dr.services.android.lib.drone.attribute.AttributeEventExtra; import com.o3dr.services.android.lib.drone.attribute.AttributeType; -import com.o3dr.services.android.lib.gcs.link.LinkEvent; import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; -import com.o3dr.services.android.lib.gcs.link.LinkEventExtra; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.drone.connection.ConnectionResult; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.mission.Mission; import com.o3dr.services.android.lib.drone.mission.action.MissionActions; import com.o3dr.services.android.lib.drone.mission.item.MissionItem; @@ -32,6 +31,8 @@ import com.o3dr.services.android.lib.drone.property.Parameter; import com.o3dr.services.android.lib.drone.property.State; import com.o3dr.services.android.lib.gcs.event.GCSEvent; +import com.o3dr.services.android.lib.gcs.link.LinkEvent; +import com.o3dr.services.android.lib.gcs.link.LinkEventExtra; import com.o3dr.services.android.lib.mavlink.MavlinkMessageWrapper; import com.o3dr.services.android.lib.model.IApiListener; import com.o3dr.services.android.lib.model.ICommandListener; @@ -652,12 +653,38 @@ public void onEndReceivingParameters() { public void onConnectionStatus(LinkConnectionStatus connectionStatus) { if (connectionStatus != null) { + switch (connectionStatus.getStatusCode()) { + case LinkConnectionStatus.FAILED: + checkForSelfRelease(); + + //This is to ensure backwards compatibility + // TODO: remove this in version 2.0 + notifyConnectionFailed(connectionStatus); + break; + } + Bundle extras = new Bundle(); extras.putParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS, connectionStatus); notifyAttributeUpdate(LinkEvent.LINK_STATE_UPDATED, extras); } } + private void notifyConnectionFailed(LinkConnectionStatus connectionStatus) { + Bundle extras = connectionStatus.getExtras(); + String msg = null; + if (extras != null) { + msg = extras.getString(LinkConnectionStatus.EXTRA_ERROR_MSG); + } + + ConnectionResult connectionResult = new ConnectionResult(0, msg); + try { + apiListener.onConnectionFailed(connectionResult); + } catch (RemoteException e) { + Timber.w(e, "Unable to forward connection fail to client."); + } + + } + @Override public void binderDied() { checkForSelfRelease(); diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java index 0372539a33..b3629eb307 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java @@ -8,7 +8,7 @@ import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.ConnectionType; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import org.droidplanner.services.android.utils.connection.WifiConnectionHandler; diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java index b9de771c78..ca48297c77 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbCDCConnection.java @@ -11,7 +11,7 @@ import com.hoho.android.usbserial.driver.UsbSerialDriver; import com.hoho.android.usbserial.driver.UsbSerialProber; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import java.io.IOException; import java.util.List; diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbConnection.java index 08fef57bfd..00e389ce69 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/usb/UsbConnection.java @@ -5,7 +5,7 @@ import android.hardware.usb.UsbManager; import android.util.Log; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import org.droidplanner.services.android.communication.connection.AndroidMavLinkConnection; import org.droidplanner.services.android.core.MAVLink.connection.MavLinkConnectionTypes; diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/model/DataLink.java b/ServiceApp/src/org/droidplanner/services/android/communication/model/DataLink.java index 11afb5cf12..62ac365510 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/model/DataLink.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/model/DataLink.java @@ -1,6 +1,6 @@ package org.droidplanner.services.android.communication.model; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import com.o3dr.services.android.lib.model.ICommandListener; public class DataLink { diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java b/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java index 58295089ac..39bbd31bcd 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java @@ -9,7 +9,7 @@ import com.google.android.gms.analytics.HitBuilders; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.ConnectionType; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import com.o3dr.services.android.lib.model.ICommandListener; import org.droidplanner.services.android.communication.connection.AndroidMavLinkConnection; diff --git a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java index 7df85bac58..bd744eee12 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java @@ -5,7 +5,7 @@ import com.MAVLink.MAVLinkPacket; import com.MAVLink.Parser; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import org.droidplanner.services.android.core.model.Logger; diff --git a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnectionListener.java b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnectionListener.java index ccc90cbb3b..75cb55e9d9 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnectionListener.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnectionListener.java @@ -1,7 +1,7 @@ package org.droidplanner.services.android.core.MAVLink.connection; import com.MAVLink.MAVLinkPacket; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; /** * Provides updates about the mavlink connection. diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java index 50475021bd..b67f273ef3 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java @@ -11,7 +11,7 @@ import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.ConnectionType; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.property.DroneAttribute; import com.o3dr.services.android.lib.drone.property.Parameter; import com.o3dr.services.android.lib.model.ICommandListener; diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java index 6f5b198560..e1d9111913 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java @@ -16,7 +16,7 @@ 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.connection.ConnectionParameter; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.property.DroneAttribute; import com.o3dr.services.android.lib.gcs.action.FollowMeActions; import com.o3dr.services.android.lib.gcs.follow.FollowType; diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java index c258382d0f..92b55d3459 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java @@ -20,7 +20,7 @@ import android.text.TextUtils; import android.widget.Toast; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import java.util.List; import java.util.concurrent.atomic.AtomicReference; diff --git a/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java b/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java index 54111dc41f..cc6a50e20a 100644 --- a/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java +++ b/ServiceApp/test/java/org/droidplanner/services/android/BasicTest.java @@ -9,7 +9,7 @@ import com.MAVLink.common.msg_command_long; import com.MAVLink.enums.MAV_CMD; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; -import com.o3dr.services.android.lib.drone.connection.LinkConnectionStatus; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import org.droidplanner.services.android.communication.model.DataLink; import org.droidplanner.services.android.core.MAVLink.MavLinkCommands; From f6ede4da64e872425f28a03cf42c8b392fbe1200 Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Thu, 11 Feb 2016 11:33:48 -0800 Subject: [PATCH 07/37] Fixed mavlink dependency --- ClientLib/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index 56b06d6f9b..027e9c6181 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -58,7 +58,7 @@ dependencies { compile "com.android.support:support-v4:${support_lib_version}" compile "com.google.android.gms:play-services-base:${play_services_version}" - compile project(':Mavlink') + debugCompile project(':Mavlink') releaseCompile files('libs/Mavlink.jar') } From 5a7d2796028abe4d678ee586fe5170705c8a2f09 Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Thu, 11 Feb 2016 11:47:47 -0800 Subject: [PATCH 08/37] Updated per comments --- .../java/com/o3dr/android/client/Drone.java | 18 +- .../client/interfaces/DroneListener.java | 4 +- .../services/android/api/DroneApi.java | 5 - .../connection/AndroidTcpConnection.java | 7 + .../connection/AndroidUdpConnection.java | 7 + .../MAVLink/connection/MavLinkConnection.java | 6 +- .../android/core/drone/DroneInterfaces.java | 218 +++++++++--------- .../core/drone/variables/HeartBeat.java | 4 +- .../o3dr/sample/hellodrone/MainActivity.java | 2 +- 9 files changed, 138 insertions(+), 133 deletions(-) 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 75bc715703..2442ec3eda 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/Drone.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/Drone.java @@ -802,19 +802,21 @@ public void run() { }); } - private void sendLinkEventToListener(final Bundle extras) { + private void sendLinkEventToListener(Bundle extras) { if (linkListener == null) { return; } if (extras != null) { - handler.post(new Runnable() { - @Override - public void run() { - LinkConnectionStatus status = extras.getParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS); - linkListener.onLinkStateUpdated(status); - } - }); + final LinkConnectionStatus status = extras.getParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS); + if (status != null) { + handler.post(new Runnable() { + @Override + public void run() { + linkListener.onLinkStateUpdated(status); + } + }); + } } } diff --git a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java index 32e0657b60..1fa92a64fb 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/DroneListener.java @@ -3,6 +3,7 @@ import android.os.Bundle; import com.o3dr.services.android.lib.drone.connection.ConnectionResult; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; /** * Created by fhuya on 11/18/14. @@ -10,8 +11,7 @@ public interface DroneListener { /** - * @deprecated Use {@link #onDroneEvent(String, Bundle)} with event - * {@link com.o3dr.services.android.lib.gcs.link.LinkEvent#LINK_STATE_UPDATED} instead. + * @deprecated Use {@link LinkListener#onLinkStateUpdated(LinkConnectionStatus)} instead. * * @param result */ diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index 93a817e783..367b9b2ffd 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -539,11 +539,6 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) { droneEvent = AttributeEvent.STATE_CONNECTING; break; - case CONNECTION_FAILED: - disconnect(); - droneEvent = AttributeEvent.HEARTBEAT_TIMEOUT; - break; - case HEARTBEAT_FIRST: Bundle heartBeatExtras = new Bundle(); heartBeatExtras.putString(AttributeEventExtra.EXTRA_VEHICLE_ID, drone.getId()); diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidTcpConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidTcpConnection.java index 13170af77d..8a57565d64 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidTcpConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidTcpConnection.java @@ -2,6 +2,8 @@ import android.content.Context; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; + import org.droidplanner.services.android.core.MAVLink.connection.TcpConnection; import org.droidplanner.services.android.core.model.Logger; @@ -38,6 +40,11 @@ protected Logger initLogger() { protected void onConnectionOpened() { AndroidTcpConnection.this.onConnectionOpened(); } + + @Override + protected void onConnectionStatus(LinkConnectionStatus connectionStatus) { + AndroidTcpConnection.this.onConnectionStatus(connectionStatus); + } }; } diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidUdpConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidUdpConnection.java index 788811fdfc..722d152cdf 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidUdpConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/AndroidUdpConnection.java @@ -3,6 +3,8 @@ import android.content.Context; import android.util.Log; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; + import org.droidplanner.services.android.core.MAVLink.connection.UdpConnection; import org.droidplanner.services.android.core.model.Logger; @@ -43,6 +45,11 @@ protected Logger initLogger() { protected void onConnectionOpened() { AndroidUdpConnection.this.onConnectionOpened(); } + + @Override + protected void onConnectionStatus(LinkConnectionStatus connectionStatus) { + AndroidUdpConnection.this.onConnectionStatus(connectionStatus); + } }; } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java index bd744eee12..857242c1e3 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/MAVLink/connection/MavLinkConnection.java @@ -283,14 +283,14 @@ protected void onConnectionOpened() { } protected void onConnectionStatus(LinkConnectionStatus connectionStatus) { + reportConnectionStatus(connectionStatus); + switch (connectionStatus.getStatusCode()) { case LinkConnectionStatus.FAILED: mLogger.logInfo(TAG, "Unable to establish connection: " + connectionStatus.getStatusCode()); disconnect(); break; } - - reportConnectionStatus(connectionStatus); } /** @@ -389,7 +389,7 @@ public void addMavLinkConnectionListener(String tag, MavLinkConnectionListener l if (getConnectionStatus() == MAVLINK_CONNECTED) { Bundle extras = new Bundle(); extras.putLong(LinkConnectionStatus.EXTRA_CONNECTION_TIME, mConnectionTime.get()); - reportConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.CONNECTED, extras)); + listener.onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.CONNECTED, extras)); } } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneInterfaces.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneInterfaces.java index ee5af02ce9..32be8546e6 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneInterfaces.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneInterfaces.java @@ -6,133 +6,132 @@ import org.droidplanner.services.android.core.MAVLink.WaypointManager; import org.droidplanner.services.android.core.drone.autopilot.Drone; -import org.droidplanner.services.android.core.drone.autopilot.MavLinkDrone; public class DroneInterfaces { - /** - * Sets of drone events used for broadcast throughout the app. - */ - public enum DroneEventsType { + /** + * Sets of drone events used for broadcast throughout the app. + */ + public enum DroneEventsType { /** * Denotes vehicle altitude change event. */ ALTITUDE, - /** + /** * */ - ORIENTATION, + ORIENTATION, - /** + /** * Denotes vehicle speed change event. */ - SPEED, + SPEED, - /** + /** * */ - BATTERY, + BATTERY, - /** + /** * */ - GUIDEDPOINT, + GUIDEDPOINT, - /** + /** * Denotes vehicle attitude change event. */ - ATTITUDE, + ATTITUDE, - /** + /** * */ - RADIO, + RADIO, - /** + /** * */ - RC_IN, + RC_IN, - /** + /** * */ - RC_OUT, + RC_OUT, - /** + /** * */ - ARMING, + ARMING, - /** + /** * */ - AUTOPILOT_WARNING, + AUTOPILOT_WARNING, - /** + /** * */ - MODE, + MODE, - /** + /** * */ - STATE, + STATE, - /** + /** * */ - MISSION_UPDATE, + MISSION_UPDATE, - /** + /** * */ - MISSION_RECEIVED, + MISSION_RECEIVED, - /** + /** * */ - TYPE, + TYPE, - /** + /** * */ - HOME, + HOME, - /** + /** * */ - CALIBRATION_IMU, + CALIBRATION_IMU, - /** + /** * */ - CALIBRATION_TIMEOUT, + CALIBRATION_TIMEOUT, - /** + /** * */ - HEARTBEAT_TIMEOUT, + HEARTBEAT_TIMEOUT, - /** + /** * */ - HEARTBEAT_FIRST, + HEARTBEAT_FIRST, - /** + /** * */ - HEARTBEAT_RESTORED, + HEARTBEAT_RESTORED, - /** + /** * */ - DISCONNECTED, + DISCONNECTED, - /** + /** * Successful connection event. */ - CONNECTED, + CONNECTED, /** * Connection initiated event. @@ -140,92 +139,87 @@ public enum DroneEventsType { CONNECTING, /** - * Connection failed event. + * */ - CONNECTION_FAILED, + MISSION_SENT, - /** + /** * */ - MISSION_SENT, + ARMING_STARTED, - /** + /** * */ - ARMING_STARTED, + INVALID_POLYGON, - /** + /** * */ - INVALID_POLYGON, + MISSION_WP_UPDATE, - /** + /** * */ - MISSION_WP_UPDATE, + WARNING_SIGNAL_WEAK, + /** + * Announces that a new version for the firmware has been received + */ + FIRMWARE, - /** - * - */ - WARNING_SIGNAL_WEAK, - /** - * Announces that a new version for the firmware has been received - */ - FIRMWARE, + /** + * Warn that the drone has no gps signal + */ + WARNING_NO_GPS, - /** - * Warn that the drone has no gps signal - */ - WARNING_NO_GPS, - - /** - * New magnetometer data has been received - */ - MAGNETOMETER, - - /** - * The drone camera footprints has been updated - */ - FOOTPRINT, + /** + * New magnetometer data has been received + */ + MAGNETOMETER, - /** - * The ekf status was updated. - */ - EKF_STATUS_UPDATE, + /** + * The drone camera footprints has been updated + */ + FOOTPRINT, + + /** + * The ekf status was updated. + */ + EKF_STATUS_UPDATE, - /** - * The horizontal position is ok, and the home position is available. - */ - EKF_POSITION_STATE_UPDATE, + /** + * The horizontal position is ok, and the home position is available. + */ + EKF_POSITION_STATE_UPDATE, - /** - * A mission item has been reached. - */ - MISSION_WP_REACHED, - } + /** + * A mission item has been reached. + */ + MISSION_WP_REACHED, + } - public interface OnDroneListener { - public void onDroneEvent(DroneEventsType event, T drone); - } + public interface OnDroneListener { + public void onDroneEvent(DroneEventsType event, T drone); + } - public interface AttributeEventListener { - void onAttributeEvent(String attributeEvent, Bundle eventInfo, boolean checkForSololinkApi); - } + public interface AttributeEventListener { + void onAttributeEvent(String attributeEvent, Bundle eventInfo, boolean checkForSololinkApi); + } - public interface OnParameterManagerListener { - public void onBeginReceivingParameters(); + public interface OnParameterManagerListener { + public void onBeginReceivingParameters(); - public void onParameterReceived(Parameter parameter, int index, int count); + public void onParameterReceived(Parameter parameter, int index, int count); - public void onEndReceivingParameters(); - } + public void onEndReceivingParameters(); + } - public interface OnWaypointManagerListener { - public void onBeginWaypointEvent(WaypointManager.WaypointEvent_Type wpEvent); + public interface OnWaypointManagerListener { + public void onBeginWaypointEvent(WaypointManager.WaypointEvent_Type wpEvent); - public void onWaypointEvent(WaypointManager.WaypointEvent_Type wpEvent, int index, int count); + public void onWaypointEvent(WaypointManager.WaypointEvent_Type wpEvent, int index, int count); - public void onEndWaypointEvent(WaypointManager.WaypointEvent_Type wpEvent); - } + public void onEndWaypointEvent(WaypointManager.WaypointEvent_Type wpEvent); + } } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/HeartBeat.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/HeartBeat.java index 242629ca26..9fc6037519 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/HeartBeat.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/HeartBeat.java @@ -103,7 +103,7 @@ public boolean isConnectionAlive() { @Override public void onDroneEvent(DroneEventsType event, MavLinkDrone drone) { switch (event) { - case CONNECTION_FAILED: + case HEARTBEAT_TIMEOUT: case DISCONNECTED: notifyDisconnected(); break; @@ -123,7 +123,7 @@ protected void onHeartbeatTimeout() { switch (heartbeatState) { case FIRST_HEARTBEAT: Timber.i("First heartbeat timeout."); - myDrone.notifyDroneEvent(DroneEventsType.CONNECTION_FAILED); + myDrone.notifyDroneEvent(DroneEventsType.HEARTBEAT_TIMEOUT); break; default: diff --git a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java index 3eb952283e..b5b4f94947 100644 --- a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java +++ b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java @@ -248,7 +248,7 @@ private void checkSoloState() { @Override public void onDroneConnectionFailed(ConnectionResult result) { - alertUser("Connection Failed:" + result.getExtras()); + alertUser("Connection Failed:" + result.getErrorCode()); } @Override From 149807487e89162d05ab73d2c23d46156391e0c1 Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Thu, 11 Feb 2016 11:58:56 -0800 Subject: [PATCH 09/37] Removed disconnect on HEARTBEAT_TIMEOUT --- .../services/android/core/drone/variables/HeartBeat.java | 1 - 1 file changed, 1 deletion(-) diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/HeartBeat.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/HeartBeat.java index 9fc6037519..cc8814d1d7 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/HeartBeat.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/variables/HeartBeat.java @@ -103,7 +103,6 @@ public boolean isConnectionAlive() { @Override public void onDroneEvent(DroneEventsType event, MavLinkDrone drone) { switch (event) { - case HEARTBEAT_TIMEOUT: case DISCONNECTED: notifyDisconnected(); break; From 0644d3fd83cbbcb8c3fb2006c02bd328e2bb0e52 Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Thu, 11 Feb 2016 14:54:16 -0800 Subject: [PATCH 10/37] Updated javadocs --- .../main/java/com/o3dr/android/client/Drone.java | 15 +++++++++++++++ .../android/client/interfaces/LinkListener.java | 15 +++++++++++++-- .../services/android/api/DroneApi.java | 2 +- 3 files changed, 29 insertions(+), 3 deletions(-) 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 2442ec3eda..d6de646b77 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/Drone.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/Drone.java @@ -396,16 +396,31 @@ private T getAttributeDefaultValue(String attributeType) } } + /** + * Connect to a vehicle using a specified {@link ConnectionParameter}. + * + * @param connParams Specified parameters to determine how to connect the vehicle. + */ public void connect(final ConnectionParameter connParams) { connect(connParams, null); } + /** + * Connect to a vehicle using a specified {@link ConnectionParameter} and a {@link LinkListener} + * callback. + * + * @param connParams Specified parameters to determine how to connect the vehicle. + * @param linkListener A callback that will update the caller on the state of the link connection. + */ public void connect(ConnectionParameter connParams, LinkListener linkListener) { VehicleApi.getApi(this).connect(connParams); this.connectionParameter = connParams; this.linkListener = linkListener; } + /** + * Disconnect from the vehicle. + */ public void disconnect() { VehicleApi.getApi(this).disconnect(); this.connectionParameter = null; diff --git a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java index d084d74a12..2dac623510 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/interfaces/LinkListener.java @@ -1,11 +1,22 @@ package com.o3dr.android.client.interfaces; +import android.support.annotation.NonNull; + +import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; /** - * Created by chavi on 2/8/16. + * An interface that will update the caller with information about the link connection. + * + * This is passed to the {@link com.o3dr.android.client.Drone#connect(ConnectionParameter, LinkListener)} + * method. */ public interface LinkListener { - void onLinkStateUpdated(LinkConnectionStatus connectionStatus); + /** + * The callback that notifies the caller about the current state of the link connection. + * + * @param connectionStatus Contains information about the connection status. + */ + void onLinkStateUpdated(@NonNull LinkConnectionStatus connectionStatus); } diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index 367b9b2ffd..a86d0bd3bf 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -653,7 +653,7 @@ public void onConnectionStatus(LinkConnectionStatus connectionStatus) { checkForSelfRelease(); //This is to ensure backwards compatibility - // TODO: remove this in version 2.0 + // TODO: remove this in version 3.0 notifyConnectionFailed(connectionStatus); break; } From 24592636f1924120d5dd4874bde8f3b8fa27bcda Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Fri, 12 Feb 2016 13:39:14 -0800 Subject: [PATCH 11/37] Updated services to support in app wifi --- .../drone/connection/ConnectionParameter.java | 150 +++++++++++++++++- .../services/android/api/DroneApi.java | 48 +++--- .../connection/SoloConnection.java | 25 +-- .../communication/service/MAVLinkClient.java | 4 +- .../android/core/drone/DroneManager.java | 2 +- .../drone/manager/MavLinkDroneManager.java | 5 +- .../services/android/utils/NetworkUtils.java | 6 +- .../connection/WifiConnectionHandler.java | 147 +++++++++++------ 8 files changed, 304 insertions(+), 83 deletions(-) diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionParameter.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionParameter.java index d53d5e6f6c..68b62f0e06 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionParameter.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/connection/ConnectionParameter.java @@ -3,6 +3,7 @@ import android.os.Bundle; import android.os.Parcel; import android.os.Parcelable; +import android.support.annotation.Nullable; import android.text.TextUtils; /** @@ -14,6 +15,149 @@ public class ConnectionParameter implements Parcelable { private final Bundle paramsBundle; private final DroneSharePrefs droneSharePrefs; + /** + * @return Returns a new {@link ConnectionParameter} with type {@link ConnectionType#TYPE_USB} + * and baud rate {@link ConnectionType#DEFAULT_USB_BAUD_RATE}. + */ + public static ConnectionParameter newUsbConnection() { + return newUsbConnection(ConnectionType.DEFAULT_USB_BAUD_RATE); + } + + /** + * + * @param usbBaudRate Baud rate for USB connection. + * + * @return Returns a new {@link ConnectionParameter} with type {@link ConnectionType#TYPE_USB}. + */ + public static ConnectionParameter newUsbConnection(int usbBaudRate) { + Bundle paramsBundle = new Bundle(1); + paramsBundle.putInt(ConnectionType.EXTRA_USB_BAUD_RATE, usbBaudRate); + + return new ConnectionParameter(ConnectionType.TYPE_USB, paramsBundle); + } + + /** + * + * @return Returns {@link ConnectionParameter} with type {@link ConnectionType#TYPE_UDP}, using + * {@link ConnectionType#DEFAULT_UDP_SERVER_PORT} port. + */ + public static ConnectionParameter newUdpConnection() { + return newUdpConnection(ConnectionType.DEFAULT_UDP_SERVER_PORT); + } + + /** + * + * @param udpPort Port for the UDP connection. + * + * @return Returns {@link ConnectionParameter} with type {@link ConnectionType#TYPE_UDP}. + */ + public static ConnectionParameter newUdpConnection(int udpPort) { + return newUdpConnection(udpPort, null, 0, null); + } + + /** + * + * @param udpPort Port for the UDP connection. + * @param udpPingReceiverIp IP address of the UDP server to ping. If this value is null, it is ignored + * along with udpPingReceiverPort and udpPingPayload. + * @param udpPingReceiverPort Port of the UDP server to ping. + * @param udpPingPayload Ping payload. + * + * @return Returns {@link ConnectionParameter} with type {@link ConnectionType#TYPE_UDP}. The ping + * period is set to {@link ConnectionType#DEFAULT_UDP_PING_PERIOD} + */ + public static ConnectionParameter newUdpConnection(int udpPort, @Nullable String udpPingReceiverIp, int udpPingReceiverPort, + byte[] udpPingPayload) { + return newUdpConnection(udpPort, udpPingReceiverIp, udpPingReceiverPort, udpPingPayload, ConnectionType.DEFAULT_UDP_PING_PERIOD); + } + + /** + * + * @param udpPort Port for the UDP connection. + * @param udpPingReceiverIp IP address of the UDP server to ping. If this value is null, it is ignored + * along with udpPingReceiverPort, udpPingPayload, and pingPeriod. + * @param udpPingReceiverPort Port of the UDP server to ping. + * @param udpPingPayload Ping payload. + * @param pingPeriod How often should the udp ping be performed. + * + * @return Returns {@link ConnectionParameter} with type {@link ConnectionType#TYPE_UDP}. + */ + public static ConnectionParameter newUdpConnection(int udpPort, @Nullable String udpPingReceiverIp, int udpPingReceiverPort, + byte[] udpPingPayload, long pingPeriod) { + Bundle paramsBundle = new Bundle(); + paramsBundle.putInt(ConnectionType.EXTRA_UDP_SERVER_PORT, udpPort); + + if (!TextUtils.isEmpty(udpPingReceiverIp)) { + paramsBundle.putString(ConnectionType.EXTRA_UDP_PING_RECEIVER_IP, udpPingReceiverIp); + paramsBundle.putInt(ConnectionType.EXTRA_UDP_PING_RECEIVER_PORT, udpPingReceiverPort); + paramsBundle.putByteArray(ConnectionType.EXTRA_UDP_PING_PAYLOAD, udpPingPayload); + paramsBundle.putLong(ConnectionType.EXTRA_UDP_PING_PERIOD, pingPeriod); + } + + return new ConnectionParameter(ConnectionType.TYPE_UDP, paramsBundle); + } + + /** + * + * @param tcpServerIp TCP server IP address. + * + * @return Returns {@link ConnectionParameter} with type {@link ConnectionType#TYPE_TCP}, using + * {@link ConnectionType#DEFAULT_TCP_SERVER_PORT}. + */ + public static ConnectionParameter newTcpConnection(String tcpServerIp) { + return newTcpConnection(tcpServerIp, ConnectionType.DEFAULT_TCP_SERVER_PORT); + } + + /** + * + * @param tcpServerIp TCP server IP address. + * @param tcpServerPort TCP server port. + * + * @return Returns {@link ConnectionParameter} with type {@link ConnectionType#TYPE_TCP}. + */ + public static ConnectionParameter newTcpConnection(String tcpServerIp, int tcpServerPort) { + Bundle paramsBundle = new Bundle(2); + paramsBundle.putString(ConnectionType.EXTRA_TCP_SERVER_IP, tcpServerIp); + paramsBundle.putInt(ConnectionType.EXTRA_TCP_SERVER_PORT, tcpServerPort); + + return new ConnectionParameter(ConnectionType.TYPE_TCP, paramsBundle); + } + + /** + * + * @param bluetoothAddress Bluetooth address. + * + * @return Returns {@link ConnectionParameter} with type {@link ConnectionType#TYPE_BLUETOOTH}. + */ + public static ConnectionParameter newBluetoothConnection(String bluetoothAddress) { + Bundle paramsBundle = new Bundle(1); + paramsBundle.putString(ConnectionType.EXTRA_BLUETOOTH_ADDRESS, bluetoothAddress); + + return new ConnectionParameter(ConnectionType.TYPE_BLUETOOTH, paramsBundle); + } + + /** + * + * @param ssid Wifi SSID of the solo vehicle link. This will remove a leading and/or trailing quotation. + * @param password Password to access the solo wifi network. This value can be null as long as the wifi + * configuration has been set up and stored in the mobile device's system. + * + * @return Returns {@link ConnectionParameter} with type {@link ConnectionType#TYPE_SOLO}. + */ + public static ConnectionParameter newSoloConnection(String ssid, @Nullable String password) { + String ssidWithoutQuotes = ssid.replaceAll("^\"|\"$", ""); + + Bundle paramsBundle = new Bundle(2); + paramsBundle.putString(ConnectionType.EXTRA_SOLO_LINK_ID, ssidWithoutQuotes); + paramsBundle.putString(ConnectionType.EXTRA_SOLO_LINK_PASSWORD, password); + + return new ConnectionParameter(ConnectionType.TYPE_SOLO, paramsBundle); + } + + /** + * @deprecated Use one of specified static methods + */ + //TODO: make this private version 3.0 public ConnectionParameter(int connectionType, Bundle paramsBundle){ this.connectionType = connectionType; this.paramsBundle = paramsBundle; @@ -111,8 +255,8 @@ public int hashCode(){ @Override public String toString() { String toString = "ConnectionParameter{" + - "connectionType=" + connectionType + - ", paramsBundle=["; + "connectionType=" + connectionType + + ", paramsBundle=["; if (paramsBundle != null && !paramsBundle.isEmpty()) { boolean isFirst = true; @@ -157,4 +301,4 @@ public ConnectionParameter[] newArray(int size) { return new ConnectionParameter[size]; } }; -} +} \ No newline at end of file diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index a86d0bd3bf..b2a5ceadd6 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -21,7 +21,6 @@ import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.drone.connection.ConnectionParameter; import com.o3dr.services.android.lib.drone.connection.ConnectionResult; -import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import com.o3dr.services.android.lib.drone.mission.Mission; import com.o3dr.services.android.lib.drone.mission.action.MissionActions; import com.o3dr.services.android.lib.drone.mission.item.MissionItem; @@ -31,6 +30,7 @@ import com.o3dr.services.android.lib.drone.property.Parameter; import com.o3dr.services.android.lib.drone.property.State; import com.o3dr.services.android.lib.gcs.event.GCSEvent; +import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; import com.o3dr.services.android.lib.gcs.link.LinkEvent; import com.o3dr.services.android.lib.gcs.link.LinkEventExtra; import com.o3dr.services.android.lib.mavlink.MavlinkMessageWrapper; @@ -188,8 +188,8 @@ private ConnectionParameter checkConnectionParameter(ConnectionParameter connPar throw new ConnectionException("Invalid connection parameters"); } - if (SoloConnection.isSoloConnection(context, connParams)) { - ConnectionParameter update = SoloConnection.getSoloConnectionParameterIfPossible(context); + if (SoloConnection.isUdpSoloConnection(context, connParams)) { + ConnectionParameter update = SoloConnection.getSoloConnectionParameterFromUdp(context); if (update != null) { return update; } @@ -199,8 +199,15 @@ private ConnectionParameter checkConnectionParameter(ConnectionParameter connPar public void connect(ConnectionParameter connParams) { try { - this.connectionParams = checkConnectionParameter(connParams); - this.droneMgr = service.connectDroneManager(this.connectionParams, ownerId, this); + connParams = checkConnectionParameter(connParams); + if (!connParams.equals(this.connectionParams)) { + if (this.droneMgr != null) { + throw new ConnectionException("Connection already started with different connection parameters"); + } + + this.connectionParams = connParams; + this.droneMgr = service.connectDroneManager(this.connectionParams, ownerId, this); + } } catch (ConnectionException e) { LinkConnectionStatus connectionStatus = LinkConnectionStatus .newFailedConnectionStatus(LinkConnectionStatus.INVALID_CREDENTIALS, e.getMessage()); @@ -211,6 +218,7 @@ public void connect(ConnectionParameter connParams) { public void disconnect() { service.disconnectDroneManager(this.droneMgr, clientInfo); + this.connectionParams = null; this.droneMgr = null; } @@ -647,21 +655,25 @@ public void onEndReceivingParameters() { } public void onConnectionStatus(LinkConnectionStatus connectionStatus) { - if (connectionStatus != null) { - switch (connectionStatus.getStatusCode()) { - case LinkConnectionStatus.FAILED: - checkForSelfRelease(); - - //This is to ensure backwards compatibility - // TODO: remove this in version 3.0 - notifyConnectionFailed(connectionStatus); - break; - } + switch (connectionStatus.getStatusCode()) { + case LinkConnectionStatus.FAILED: + disconnect(); + checkForSelfRelease(); - Bundle extras = new Bundle(); - extras.putParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS, connectionStatus); - notifyAttributeUpdate(LinkEvent.LINK_STATE_UPDATED, extras); + //This is to ensure backwards compatibility + // TODO: remove this in version 3.0 + notifyConnectionFailed(connectionStatus); + break; + case LinkConnectionStatus.DISCONNECTED: + disconnect(); + checkForSelfRelease(); + break; } + + Bundle extras = new Bundle(); + extras.putParcelable(LinkEventExtra.EXTRA_CONNECTION_STATUS, connectionStatus); + notifyAttributeUpdate(LinkEvent.LINK_STATE_UPDATED, extras); + } private void notifyConnectionFailed(LinkConnectionStatus connectionStatus) { diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java b/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java index b3629eb307..cee0bbdee6 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/connection/SoloConnection.java @@ -118,8 +118,10 @@ public void onWifiConnecting() { } @Override - public void onWifiDisconnected() { - onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.DISCONNECTED, null)); + public void onWifiDisconnected(String prevSsid) { + if (prevSsid.equalsIgnoreCase(soloLinkId)) { + onConnectionStatus(new LinkConnectionStatus(LinkConnectionStatus.DISCONNECTED, null)); + } } @Override @@ -127,6 +129,11 @@ public void onWifiScanResultsAvailable(List results) { checkScanResults(results); } + @Override + public void onWifiConnectionFailed(LinkConnectionStatus connectionStatus) { + onConnectionStatus(connectionStatus); + } + private void checkScanResults(List results) { if (!isConnecting()) return; @@ -146,8 +153,9 @@ private void checkScanResults(List results) { try { int connectionResult = wifiHandler.connectToWifi(targetResult, soloLinkPassword); if (connectionResult != 0) { + @LinkConnectionStatus.FailureCode int failureCode = connectionResult; LinkConnectionStatus connectionStatus = LinkConnectionStatus - .newFailedConnectionStatus(connectionResult, "Unable to connect to the target wifi " + soloLinkId); + .newFailedConnectionStatus(failureCode, "Unable to connect to the target wifi " + soloLinkId); onConnectionStatus(connectionStatus); } } catch (IllegalArgumentException e) { @@ -165,15 +173,12 @@ private boolean isConnecting() { return getConnectionStatus() == MAVLINK_CONNECTING; } - public static boolean isSoloConnection(Context context, ConnectionParameter connParam){ + public static boolean isUdpSoloConnection(Context context, ConnectionParameter connParam){ if(connParam == null) return false; final int connectionType = connParam.getConnectionType(); switch(connectionType){ - case ConnectionType.TYPE_SOLO: - return true; - case ConnectionType.TYPE_UDP: Bundle paramsBundle = connParam.getParamsBundle(); if(paramsBundle == null) @@ -188,15 +193,13 @@ public static boolean isSoloConnection(Context context, ConnectionParameter conn } } - public static ConnectionParameter getSoloConnectionParameterIfPossible(Context context){ + public static ConnectionParameter getSoloConnectionParameterFromUdp(Context context){ if(context == null) return null; final String wifiSsid = WifiConnectionHandler.getCurrentWifiLink((WifiManager) context.getSystemService(Context.WIFI_SERVICE)); if(WifiConnectionHandler.isSoloWifi(wifiSsid)){ - Bundle paramsBundle = new Bundle(); - paramsBundle.putString(ConnectionType.EXTRA_SOLO_LINK_ID, wifiSsid); - return new ConnectionParameter(ConnectionType.TYPE_SOLO, paramsBundle); + return ConnectionParameter.newSoloConnection(wifiSsid, null); } return null; diff --git a/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java b/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java index 39bbd31bcd..318295ca67 100644 --- a/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java +++ b/ServiceApp/src/org/droidplanner/services/android/communication/service/MAVLinkClient.java @@ -59,6 +59,8 @@ public void onReceivePacket(final MAVLinkPacket packet) { @Override public void onConnectionStatus(final LinkConnectionStatus connectionStatus) { + listener.onConnectionStatus(connectionStatus); + switch (connectionStatus.getStatusCode()) { case LinkConnectionStatus.DISCONNECTED: closeConnection(); @@ -74,8 +76,6 @@ public void onConnectionStatus(final LinkConnectionStatus connectionStatus) { } break; } - - listener.onConnectionStatus(connectionStatus); } }; diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java index b67f273ef3..7eb4b88c75 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/DroneManager.java @@ -132,7 +132,7 @@ public void disconnect(DroneApi.ClientInfo clientInfo) { } protected void doDisconnect(String appId, DroneApi listener) { - if (listener != null) { + if (isConnected() && listener != null) { listener.onDroneEvent(DroneInterfaces.DroneEventsType.DISCONNECTED, drone); } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java index e1d9111913..ea56e7c642 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/manager/MavLinkDroneManager.java @@ -184,8 +184,9 @@ protected void doDisconnect(String appId, DroneApi listener) { if (listener != null) { mavClient.removeLoggingFile(appId); - - listener.onDroneEvent(DroneInterfaces.DroneEventsType.DISCONNECTED, drone); + if (isConnected()) { + listener.onDroneEvent(DroneInterfaces.DroneEventsType.DISCONNECTED, drone); + } } if (mavClient.isConnected() && connectedApps.isEmpty()) { diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/NetworkUtils.java b/ServiceApp/src/org/droidplanner/services/android/utils/NetworkUtils.java index 764895947b..752be83154 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/NetworkUtils.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/NetworkUtils.java @@ -41,6 +41,10 @@ public static boolean isOnSololinkNetwork(Context context) { return true; final String connectedSSID = getCurrentWifiLink(context); - return connectedSSID != null && connectedSSID.startsWith(SoloComp.SOLO_LINK_WIFI_PREFIX); + return isSoloNetwork(connectedSSID); + } + + public static boolean isSoloNetwork(String ssid) { + return ssid != null && ssid.startsWith(SoloComp.SOLO_LINK_WIFI_PREFIX); } } diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java index b77b04ccaf..d29beb1b0d 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java @@ -13,6 +13,7 @@ import android.net.NetworkInfo; import android.net.NetworkRequest; import android.net.wifi.ScanResult; +import android.net.wifi.SupplicantState; import android.net.wifi.WifiConfiguration; import android.net.wifi.WifiInfo; import android.net.wifi.WifiManager; @@ -22,6 +23,8 @@ import com.o3dr.services.android.lib.gcs.link.LinkConnectionStatus; +import org.droidplanner.services.android.utils.NetworkUtils; + import java.util.List; import java.util.concurrent.atomic.AtomicReference; @@ -37,9 +40,11 @@ public interface WifiConnectionListener { void onWifiConnecting(); - void onWifiDisconnected(); + void onWifiDisconnected(String prevConnectedSsid); void onWifiScanResultsAvailable(List results); + + void onWifiConnectionFailed(LinkConnectionStatus connectionStatus); } public static final String SOLO_LINK_WIFI_PREFIX = "SoloLink_"; @@ -50,6 +55,7 @@ public interface WifiConnectionListener { intentFilter.addAction(WifiManager.SCAN_RESULTS_AVAILABLE_ACTION); intentFilter.addAction(WifiManager.NETWORK_STATE_CHANGED_ACTION); intentFilter.addAction(WifiManager.WIFI_STATE_CHANGED_ACTION); + intentFilter.addAction(WifiManager.SUPPLICANT_STATE_CHANGED_ACTION); } private final BroadcastReceiver broadcastReceiver = new BroadcastReceiver() { @@ -62,11 +68,28 @@ public void onReceive(Context context, Intent intent) { notifyWifiScanResultsAvailable(wifiMgr.getScanResults()); break; + case WifiManager.SUPPLICANT_STATE_CHANGED_ACTION: + SupplicantState supState = intent.getParcelableExtra(WifiManager.EXTRA_NEW_STATE); + String ssid = NetworkUtils.getCurrentWifiLink(context); + + int supplicationError = intent.getIntExtra(WifiManager.EXTRA_SUPPLICANT_ERROR, -1); + Timber.d("Supplicant state changed error %s with state %s and ssid %s", supplicationError, supState, ssid); + if (supplicationError == WifiManager.ERROR_AUTHENTICATING) { + if (NetworkUtils.isSoloNetwork(ssid)) { + notifyWifiConnectionFailed(ssid); + WifiConfiguration wifiConfig = getWifiConfigs(ssid); + if (wifiConfig != null) { + wifiMgr.removeNetwork(wifiConfig.networkId); + } + } + } + break; + case WifiManager.NETWORK_STATE_CHANGED_ACTION: NetworkInfo netInfo = intent.getParcelableExtra(WifiManager.EXTRA_NETWORK_INFO); NetworkInfo.State networkState = netInfo == null - ? NetworkInfo.State.DISCONNECTED - : netInfo.getState(); + ? NetworkInfo.State.DISCONNECTED + : netInfo.getState(); switch (networkState) { case CONNECTED: @@ -92,6 +115,13 @@ public void onReceive(Context context, Intent intent) { break; case CONNECTING: + if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.LOLLIPOP) { + NetworkInfo.DetailedState detailedState = netInfo.getDetailedState(); + if (detailedState != null && detailedState == NetworkInfo.DetailedState.VERIFYING_POOR_LINK) { + String connectingSsid = NetworkUtils.getCurrentWifiLink(context); + setDefaultNetworkIfNecessary(connectingSsid); + } + } Timber.d("Connecting to wifi network."); notifyWifiConnecting(); break; @@ -123,12 +153,12 @@ public WifiConnectionHandler(Context context) { if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.LOLLIPOP) { netReq = new NetworkRequest.Builder() - .addCapability(NetworkCapabilities.NET_CAPABILITY_TRUSTED) - .addCapability(NetworkCapabilities.NET_CAPABILITY_INTERNET) - .addCapability(NetworkCapabilities.NET_CAPABILITY_NOT_RESTRICTED) - .addCapability(NetworkCapabilities.NET_CAPABILITY_NOT_VPN) - .addTransportType(NetworkCapabilities.TRANSPORT_WIFI) - .build(); + .addCapability(NetworkCapabilities.NET_CAPABILITY_TRUSTED) + .addCapability(NetworkCapabilities.NET_CAPABILITY_INTERNET) + .addCapability(NetworkCapabilities.NET_CAPABILITY_NOT_RESTRICTED) + .addCapability(NetworkCapabilities.NET_CAPABILITY_NOT_VPN) + .addTransportType(NetworkCapabilities.TRANSPORT_WIFI) + .build(); netReqCb = new ConnectivityManager.NetworkCallback() { @@ -220,18 +250,18 @@ public void stop() { private void resetNetworkBindings(ConnectivityManager.NetworkCallback netCb) { Timber.i("Unregistering network callbacks."); - connectedSoloWifi.set(""); - try { - connMgr.unregisterNetworkCallback(netCb); - } catch (IllegalArgumentException e) { - Timber.w(e, "Network callback was not registered."); - } + connectedSoloWifi.set(NetworkUtils.getCurrentWifiLink(context)); + try { + connMgr.unregisterNetworkCallback(netCb); + } catch (IllegalArgumentException e) { + Timber.w(e, "Network callback was not registered."); + } - if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.M) { - connMgr.bindProcessToNetwork(null); - } else { - ConnectivityManager.setProcessDefaultNetwork(null); - } + if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.M) { + connMgr.bindProcessToNetwork(null); + } else { + ConnectivityManager.setProcessDefaultNetwork(null); + } } /** @@ -239,8 +269,9 @@ private void resetNetworkBindings(ConnectivityManager.NetworkCallback netCb) { */ public boolean refreshWifiAPs() { Timber.d("Querying wifi access points."); - if (wifiMgr == null) + if (wifiMgr == null) { return false; + } if (!wifiMgr.isWifiEnabled() && !wifiMgr.setWifiEnabled(true)) { Toast.makeText(context, "Unable to activate Wi-Fi!", Toast.LENGTH_LONG).show(); @@ -251,15 +282,17 @@ public boolean refreshWifiAPs() { } public boolean isOnNetwork(String wifiSsid) { - if (TextUtils.isEmpty(wifiSsid)) + if (TextUtils.isEmpty(wifiSsid)) { throw new IllegalArgumentException("Invalid wifi ssid " + wifiSsid); + } return wifiSsid.equalsIgnoreCase(getCurrentWifiLink()); } public boolean isConnected(String wifiSSID) { - if (!isOnNetwork(wifiSSID)) + if (!isOnNetwork(wifiSSID)) { return false; + } if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.LOLLIPOP) { Network network; @@ -269,8 +302,9 @@ public boolean isConnected(String wifiSSID) { network = ConnectivityManager.getProcessDefaultNetwork(); } - if (network == null) + if (network == null) { return false; + } NetworkCapabilities netCapabilities = connMgr.getNetworkCapabilities(network); return netCapabilities != null && netCapabilities.hasTransport(NetworkCapabilities.TRANSPORT_WIFI); @@ -284,8 +318,9 @@ public List getScanResults() { } public int connectToWifi(String soloLinkId, String password) { - if (TextUtils.isEmpty(soloLinkId)) + if (TextUtils.isEmpty(soloLinkId)) { return LinkConnectionStatus.INVALID_CREDENTIALS; + } ScanResult targetScanResult = null; final List scanResults = wifiMgr.getScanResults(); @@ -305,8 +340,9 @@ public int connectToWifi(String soloLinkId, String password) { } public int connectToWifi(ScanResult scanResult, String password) { - if (scanResult == null) + if (scanResult == null) { return LinkConnectionStatus.LINK_UNAVAILABLE; + } Timber.d("Connecting to wifi " + scanResult.SSID); @@ -328,11 +364,13 @@ public int connectToWifi(ScanResult scanResult, String password) { //Network is not configured and needs a password to connect if (wifiConfig == null) { Timber.d("Connecting to closed wifi network."); - if (TextUtils.isEmpty(password)) + if (TextUtils.isEmpty(password)) { return LinkConnectionStatus.INVALID_CREDENTIALS; + } - if (!connectToClosedWifi(scanResult, password)) + if (!connectToClosedWifi(scanResult, password)) { return LinkConnectionStatus.UNKNOWN; + } wifiMgr.saveConfiguration(); wifiConfig = getWifiConfigs(scanResult.SSID); @@ -347,6 +385,10 @@ public int connectToWifi(ScanResult scanResult, String password) { private WifiConfiguration getWifiConfigs(String networkSSID) { List networks = wifiMgr.getConfiguredNetworks(); + if (networks == null) { + return null; + } + for (WifiConfiguration current : networks) { if (current.SSID != null && current.SSID.equals("\"" + networkSSID + "\"")) { return current; @@ -371,8 +413,9 @@ private boolean connectToClosedWifi(ScanResult scanResult, String password) { } private static String trimWifiSsid(String wifiSsid) { - if (TextUtils.isEmpty(wifiSsid)) + if (TextUtils.isEmpty(wifiSsid)) { return ""; + } return wifiSsid.replace("\"", ""); } @@ -393,24 +436,26 @@ public static boolean isSoloWifi(String wifiSsid) { private void setDefaultNetworkIfNecessary(String wifiSsid) { final String trimmedSsid = trimWifiSsid(wifiSsid); - if (isConnected(wifiSsid)) { - notifyWifiConnected(wifiSsid); - return; - } - if (isSoloWifi(trimmedSsid)) { - //Attempt to connect to the vehicle. - Timber.i("Requesting route to sololink network"); - if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.LOLLIPOP) { - if(!trimmedSsid.equals(connectedSoloWifi.get())) { + if (!trimmedSsid.equals(connectedSoloWifi.get())) { + if (isConnected(wifiSsid)) { + notifyWifiConnected(wifiSsid); + return; + } + + if (isSoloWifi(trimmedSsid)) { + //Attempt to connect to the vehicle. + Timber.i("Requesting route to sololink network"); + if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.LOLLIPOP) { connMgr.requestNetwork((NetworkRequest) netReq, (ConnectivityManager.NetworkCallback) netReqCb); - connectedSoloWifi.set(trimmedSsid); + } else { + notifyWifiConnected(trimmedSsid); + } } else { notifyWifiConnected(trimmedSsid); } - } else { - notifyWifiConnected(trimmedSsid); + connectedSoloWifi.set(trimmedSsid); } } @@ -421,17 +466,29 @@ private void notifyWifiConnected(String wifiSsid) { } private void notifyWifiConnecting() { - if (listener != null) + if (listener != null) { listener.onWifiConnecting(); + } } private void notifyWifiDisconnected() { - if (listener != null) - listener.onWifiDisconnected(); + if (listener != null) { + listener.onWifiDisconnected(connectedSoloWifi.get()); + } + connectedSoloWifi.set(""); } private void notifyWifiScanResultsAvailable(List results) { - if (listener != null) + if (listener != null) { listener.onWifiScanResultsAvailable(results); + } + } + + private void notifyWifiConnectionFailed(String ssid) { + if (listener != null) { + LinkConnectionStatus linkConnectionStatus = LinkConnectionStatus + .newFailedConnectionStatus(LinkConnectionStatus.INVALID_CREDENTIALS, null); + listener.onWifiConnectionFailed(linkConnectionStatus); + } } } From cc696008bb6e6575d840b9ee750542ab567de1c2 Mon Sep 17 00:00:00 2001 From: KiBa1215 <446592760@qq.com> Date: Tue, 16 Feb 2016 16:30:55 +0800 Subject: [PATCH 12/37] Receiving PX4 parameters supported --- .../assets/Parameters/ParameterMetaData.xml | 4124 +++++++++++++++++ .../core/drone/autopilot/px4/Px4Native.java | 39 + .../core/drone/profiles/ParameterManager.java | 53 +- .../drone/profiles/ParameterMetadataPX4.java | 75 + .../android/core/firmware/FirmwareType.java | 2 +- .../file/IO/ParameterMetadataLoaderPX4.java | 205 + 6 files changed, 4485 insertions(+), 13 deletions(-) create mode 100644 ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterMetadataPX4.java create mode 100644 ServiceApp/src/org/droidplanner/services/android/utils/file/IO/ParameterMetadataLoaderPX4.java diff --git a/ServiceApp/assets/Parameters/ParameterMetaData.xml b/ServiceApp/assets/Parameters/ParameterMetaData.xml index d933aa5806..805312d8ba 100644 --- a/ServiceApp/assets/Parameters/ParameterMetaData.xml +++ b/ServiceApp/assets/Parameters/ParameterMetaData.xml @@ -7457,4 +7457,4128 @@ ../libraries/APM_Control/AP_SteerController.cpp + + 3 + + + Speed controller bandwidth + Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness. + Hertz + 10 + 250 + + + Reverse direction + Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction. + 0 + 1 + + + Speed (RPM) controller gain + Speed (RPM) controller gain. Determines controller + aggressiveness; units are amp-seconds per radian. Systems with + higher rotational inertia (large props) will need gain increased; + systems with low rotational inertia (small props) may need gain + decreased. Higher values result in faster response, but may result + in oscillation and excessive overshoot. Lower values result in a + slower, smoother response. + amp-seconds per radian + 3 + 0.00 + 1.00 + + + Idle speed (e Hz) + Idle speed (e Hz) + Hertz + 3 + 0.0 + 100.0 + + + Spin-up rate (e Hz/s) + Spin-up rate (e Hz/s) + Hz/s + 5 + 1000 + + + Index of this ESC in throttle command messages. + Index of this ESC in throttle command messages. + Index + 0 + 15 + + + Extended status ID + Extended status ID + 1 + 1000000 + + + Extended status interval (µs) + Extended status interval (µs) + µs + 0 + 1000000 + + + ESC status interval (µs) + ESC status interval (µs) + µs + 1000000 + + + Motor current limit in amps + Motor current limit in amps. This determines the maximum + current controller setpoint, as well as the maximum allowable + current setpoint slew rate. This value should generally be set to + the continuous current rating listed in the motor’s specification + sheet, or set equal to the motor’s specified continuous power + divided by the motor voltage limit. + Amps + 3 + 1 + 80 + + + Motor Kv in RPM per volt + Motor Kv in RPM per volt. This can be taken from the motor’s + specification sheet; accuracy will help control performance but + some deviation from the specified value is acceptable. + RPM/v + 0 + 4000 + + + READ ONLY: Motor inductance in henries. + READ ONLY: Motor inductance in henries. This is measured on start-up. + henries + 3 + + + Number of motor poles. + Number of motor poles. Used to convert mechanical speeds to + electrical speeds. This number should be taken from the motor’s + specification sheet. + Poles + 2 + 40 + + + READ ONLY: Motor resistance in ohms + READ ONLY: Motor resistance in ohms. This is measured on start-up. When + tuning a new motor, check that this value is approximately equal + to the value shown in the motor’s specification sheet. + Ohms + 3 + + + Acceleration limit (V) + Acceleration limit (V) + Volts + 3 + 0.01 + 1.00 + + + Motor voltage limit in volts + Motor voltage limit in volts. The current controller’s + commanded voltage will never exceed this value. Note that this may + safely be above the nominal voltage of the motor; to determine the + actual motor voltage limit, divide the motor’s rated power by the + motor current limit. + Volts + 3 + 0 + + + + + Body angular rate process noise + + + Body angular acceleration process noise + + + Acceleration process noise + + + Magnet field vector process noise + + + Gyro measurement noise + + + Accel measurement noise + + + Mag measurement noise + + + Moment of inertia matrix diagonal entry (1, 1) + kg*m^2 + + + Moment of inertia matrix diagonal entry (2, 2) + kg*m^2 + + + Moment of inertia matrix diagonal entry (3, 3) + kg*m^2 + + + Moment of inertia enabled in estimator + If set to != 0 the moment of inertia will be used in the estimator + 0 + 1 + + + + + Complimentary filter accelerometer weight + 0 + 1 + 2 + + + Complimentary filter magnetometer weight + 0 + 1 + 2 + + + Complimentary filter external heading weight + 0 + 1 + + + Complimentary filter gyroscope bias weight + 0 + 1 + 2 + + + Magnetic declination, in degrees + This parameter is not used in normal operation, as the declination is looked up based on the GPS coordinates of the vehicle. + degrees + 2 + + + Enable automatic GPS based declination compensation + 0 + 1 + + + External heading usage mode (from Motion capture/Vision) + Set to 1 to use heading estimate from vision. + Set to 2 to use heading from motion capture + 0 + 2 + + + Enable acceleration compensation based on GPS + velocity + 1 + 2 + + + Gyro bias limit + 0 + 2 + rad/s + 3 + + + Threshold (of RMS) to warn about high vibration levels + 0.01 + 10 + 2 + + + + + Scaling factor for battery voltage sensor on PX4IO + 1 + 100000 + + + Scaling factor for battery voltage sensor on FMU v2 + + + Scaling factor for battery current sensor + + + Empty cell voltage + Defines the voltage where a single cell of the battery is considered empty. + V + 2 + + + Full cell voltage + Defines the voltage where a single cell of the battery is considered full. + V + 2 + + + Voltage drop per cell on 100% load + This implicitely defines the internal resistance to maximum current ratio and assumes linearity. + 0.0 + 1.5 + V + 2 + + + Number of cells + Defines the number of cells the attached battery consists of. + 1 + 10 + S + + + Battery capacity + Defines the capacity of the attached battery. + mA + 0 + + + + + Camera trigger interval + This parameter sets the time between two consecutive trigger events + 4.0 + 10000.0 + milliseconds + + + Camera trigger polarity + This parameter sets the polarity of the trigger (0 = ACTIVE_LOW, 1 = ACTIVE_HIGH ) + 0 + 1 + + + Camera trigger activation time + This parameter sets the time the trigger needs to pulled high or low. + milliseconds + + + Camera trigger mode + 0 disables the trigger, 1 sets it to enabled on command, 2 always on + 0 + 2 + + + Camera trigger pin + Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6) + 1 + 123456 + + + + + Circuit breaker for power supply check + Setting this parameter to 894281 will disable the power valid checks in the commander. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 894281 + + + Circuit breaker for rate controller output + Setting this parameter to 140253 will disable the rate controller uORB publication. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 140253 + + + Circuit breaker for IO safety + Setting this parameter to 22027 will disable IO safety. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 22027 + + + Circuit breaker for airspeed sensor + Setting this parameter to 162128 will disable the check for an airspeed sensor. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 162128 + + + Circuit breaker for flight termination + Setting this parameter to 121212 will disable the flight termination action. --> The IO driver will not do flight termination if requested by the FMU WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 121212 + + + Circuit breaker for engine failure detection + Setting this parameter to 284953 will disable the engine failure detection. If the aircraft is in engine failure mode the enine failure flag will be set to healthy WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 284953 + + + Circuit breaker for GPS failure detection + Setting this parameter to 240024 will disable the GPS failure detection. If this check is enabled, then the sensor check will fail if the GPS module is missing. It will also check for excessive signal noise on the GPS receiver and warn the user if detected. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 240024 + + + Circuit breaker for disabling buzzer + Setting this parameter to 782097 will disable the buzzer audio notification. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 782097 + + + Circuit breaker for USB link check + Setting this parameter to 197848 will disable the USB connected checks in the commander. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 197848 + + + + + Datalink loss mode enabled + Set to 1 to enable actions triggered when the datalink is lost. + 0 + 1 + + + Datalink loss time threshold + After this amount of seconds without datalink the data link lost mode triggers + 0 + 30 + second + + + Datalink regain time threshold + After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss' flag is set back to false + 0 + 30 + second + + + Engine Failure Throttle Threshold + Engine failure triggers only above this throttle value + 0.0 + 1.0 + 1 + + + Engine Failure Current/Throttle Threshold + Engine failure triggers only below this current value + 0.0 + 30.0 + ampere + 2 + + + Engine Failure Time Threshold + Engine failure triggers only if the throttle threshold and the current to throttle threshold are violated for this time + 0.0 + 60.0 + second + 1 + + + RC loss time threshold + After this amount of seconds without RC connection the rc lost flag is set to true + 0 + 35 + second + 1 + + + Home set horizontal threshold + The home position will be set if the estimated positioning accuracy is below the threshold. + 2 + 15 + meter + 2 + + + Home set vertical threshold + The home position will be set if the estimated positioning accuracy is below the threshold. + 5 + 25 + meter + 2 + + + Autosaving of params + If not equal to zero the commander will automatically save parameters to persistent storage once changed. Default is on, as the interoperability with currently deployed GCS solutions depends on parameters being sticky. Developers can default it to off. + 0 + 1 + + + RC control input mode + The default value of 0 requires a valid RC transmitter setup. Setting this to 1 disables RC input handling and the associated checks. A value of 2 will generate RC control data from manual input received via MAVLink instead of directly forwarding the manual input data. + 0 + 2 + + + Time-out for auto disarm after landing + A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be automatically disarmed in case a landing situation has been detected during this period. A value of zero means that automatic disarming is disabled. + 0 + + + + + Airfield home Lat + Latitude of airfield home waypoint + -900000000 + 900000000 + degrees * 1e7 + + + Airfield home Lon + Longitude of airfield home waypoint + -1800000000 + 1800000000 + degrees * 1e7 + + + Airfield home alt + Altitude of airfield home waypoint + -50 + m + + + Comms hold wait time + The amount of time in seconds the system should wait at the comms hold waypoint + 0.0 + seconds + + + Comms hold Lat + Latitude of comms hold waypoint + -900000000 + 900000000 + degrees * 1e7 + + + Comms hold Lon + Longitude of comms hold waypoint + -1800000000 + 1800000000 + degrees * 1e7 + + + Comms hold alt + Altitude of comms hold waypoint + -50 + 30000 + m + + + Airfield hole wait time + The amount of time in seconds the system should wait at the airfield home waypoint + 0.0 + seconds + + + Number of allowed Datalink timeouts + After more than this number of data link timeouts the aircraft returns home directly + 0 + 1000 + + + Skip comms hold wp + If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to airfield home + 0 + 1 + + + + + Attitude Roll Time Constant + This defines the latency between a roll step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. + 0.4 + 1.0 + seconds + + + Attitude Pitch Time Constant + This defines the latency between a pitch step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. + 0.2 + 1.0 + seconds + + + Pitch rate proportional gain + This defines how much the elevator input will be commanded depending on the current body angular rate error. + 0.005 + 1.0 + + + Pitch rate integrator gain + This gain defines how much control response will result out of a steady state error. It trims any constant error. + 0.005 + 0.5 + + + Maximum positive / up pitch rate + This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Maximum negative / down pitch rate + This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Pitch rate integrator limit + The portion of the integrator part in the control surface deflection is limited to this value + 0.0 + 1.0 + + + Roll rate proportional Gain + This defines how much the aileron input will be commanded depending on the current body angular rate error. + 0.005 + 1.0 + + + Roll rate integrator Gain + This gain defines how much control response will result out of a steady state error. It trims any constant error. + 0.005 + 0.2 + + + Roll Integrator Anti-Windup + The portion of the integrator part in the control surface deflection is limited to this value. + 0.0 + 1.0 + + + Maximum Roll Rate + This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Yaw rate proportional gain + This defines how much the rudder input will be commanded depending on the current body angular rate error. + 0.005 + 1.0 + + + Yaw rate integrator gain + This gain defines how much control response will result out of a steady state error. It trims any constant error. + 0.0 + 50.0 + + + Yaw rate integrator limit + The portion of the integrator part in the control surface deflection is limited to this value + 0.0 + 1.0 + + + Maximum Yaw Rate + This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Wheel steering rate proportional gain + This defines how much the wheel steering input will be commanded depending on the current body angular rate error. + 0.005 + 1.0 + + + Wheel steering rate integrator gain + This gain defines how much control response will result out of a steady state error. It trims any constant error. + 0.0 + 50.0 + + + Wheel steering rate integrator limit + The portion of the integrator part in the control surface deflection is limited to this value + 0.0 + 1.0 + + + Maximum wheel steering rate + This limits the maximum wheel steering rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Roll rate feed forward + Direct feed forward from rate setpoint to control surface output. Use this to obtain a tigher response of the controller without introducing noise amplification. + 0.0 + 10.0 + + + Pitch rate feed forward + Direct feed forward from rate setpoint to control surface output + 0.0 + 10.0 + + + Yaw rate feed forward + Direct feed forward from rate setpoint to control surface output + 0.0 + 10.0 + + + Wheel steering rate feed forward + Direct feed forward from rate setpoint to control surface output + 0.0 + 10.0 + + + Minimal speed for yaw coordination + For airspeeds above this value, the yaw rate is calculated for a coordinated turn. Set to a very high value to disable. + m/s + + + Method used for yaw coordination + The param value sets the method used to calculate the yaw rate 0: open-loop zero lateral acceleration based on kinematic constraints 1: closed-loop: try to reduce lateral acceleration to 0 by measuring the acceleration + 0 + 1 + m/s + + + Minimum Airspeed + If the airspeed falls below this value, the TECS controller will try to increase airspeed more aggressively. + 0.0 + 40 + m/s + + + Trim Airspeed + The TECS controller tries to fly at this airspeed. + 0.0 + 40 + m/s + + + Maximum Airspeed + If the airspeed is above this value, the TECS controller will try to decrease airspeed more aggressively. + 0.0 + 40 + m/s + + + Roll Setpoint Offset + An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe. + -90.0 + 90.0 + deg + + + Pitch Setpoint Offset + An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe. + -90.0 + 90.0 + deg + + + Max Manual Roll + Max roll for manual control in attitude stabilized mode + 0.0 + 90.0 + deg + + + Max Manual Pitch + Max pitch for manual control in attitude stabilized mode + 0.0 + 90.0 + deg + + + Scale factor for flaps + 0.0 + 1.0 + + + Scale factor for flaperons + 0.0 + 1.0 + + + + + Minimum descent rate + This is the sink rate of the aircraft with the throttle set to THR_MIN and flown at the same airspeed as used to measure FW_T_CLMB_MAX. + + + Maximum descent rate + This sets the maximum descent rate that the controller will use. If this value is too large, the aircraft can over-speed on descent. This should be set to a value that can be achieved without exceeding the lower pitch angle limit and without over-speeding the aircraft. + + + TECS time constant + This is the time constant of the TECS control algorithm (in seconds). Smaller values make it faster to respond, larger values make it slower to respond. + + + TECS Throttle time constant + This is the time constant of the TECS throttle control algorithm (in seconds). Smaller values make it faster to respond, larger values make it slower to respond. + + + Throttle damping factor + This is the damping gain for the throttle demand loop. Increase to add damping to correct for oscillations in speed and height. + + + Integrator gain + This is the integrator gain on the control loop. Increasing this gain increases the speed at which speed and height offsets are trimmed out, but reduces damping and increases overshoot. + + + Maximum vertical acceleration + This is the maximum vertical acceleration (in metres/second square) either up or down that the controller will use to correct speed or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) allows for reasonably aggressive pitch changes if required to recover from under-speed conditions. + + + Complementary filter "omega" parameter for height + This is the cross-over frequency (in radians/second) of the complementary filter used to fuse vertical acceleration and barometric height to obtain an estimate of height rate and height. Increasing this frequency weights the solution more towards use of the barometer, whilst reducing it weights the solution more towards use of the accelerometer data. + + + Complementary filter "omega" parameter for speed + This is the cross-over frequency (in radians/second) of the complementary filter used to fuse longitudinal acceleration and airspeed to obtain an improved airspeed estimate. Increasing this frequency weights the solution more towards use of the arispeed sensor, whilst reducing it weights the solution more towards use of the accelerometer data. + + + Roll -> Throttle feedforward + Increasing this gain turn increases the amount of throttle that will be used to compensate for the additional drag created by turning. Ideally this should be set to approximately 10 x the extra sink rate in m/s created by a 45 degree bank turn. Increase this gain if the aircraft initially loses energy in turns and reduce if the aircraft initially gains energy in turns. Efficient high aspect-ratio aircraft (eg powered sailplanes) can use a lower value, whereas inefficient low aspect-ratio models (eg delta wings) can use a higher value. + + + Speed <--> Altitude priority + This parameter adjusts the amount of weighting that the pitch control applies to speed vs height errors. Setting it to 0.0 will cause the pitch control to control height and ignore speed errors. This will normally improve height accuracy but give larger airspeed errors. Setting it to 2.0 will cause the pitch control loop to control speed and ignore height errors. This will normally reduce airspeed errors, but give larger height errors. The default value of 1.0 allows the pitch control to simultaneously control height and speed. Note to Glider Pilots - set this parameter to 2.0 (The glider will adjust its pitch angle to maintain airspeed, ignoring changes in height). + + + Pitch damping factor + This is the damping gain for the pitch demand loop. Increase to add damping to correct for oscillations in height. The default value of 0.0 will work well provided the pitch to servo controller has been tuned properly. + + + Height rate P factor + + + Height rate FF factor + + + Speed rate P factor + + + + + Loiter time + The amount of time in seconds the system should do open loop loiter and wait for gps recovery before it goes into flight termination. + 0.0 + seconds + + + Open loop loiter roll + Roll in degrees during the open loop loiter + 0.0 + 30.0 + deg + + + Open loop loiter pitch + Pitch in degrees during the open loop loiter + -30.0 + 30.0 + deg + + + Open loop loiter thrust + Thrust value which is set during the open loop loiter + 0.0 + 1.0 + + + + + Geofence violation action + 0 = none, 1 = warning (default), 2 = loiter, 3 = return to launch, 4 = fight termination + 0 + 4 + + + Geofence altitude mode + Select which altitude reference should be used 0 = WGS84, 1 = AMSL + 0 + 1 + + + Geofence source + Select which position source should be used. Selecting GPS instead of global position makes sure that there is no dependence on the position estimator 0 = global position, 1 = GPS + 0 + 1 + + + Geofence counter limit + Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered + -1 + 10 + + + Max horizontal distance in meters + Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value. + -1 + meters + + + Max vertical distance in meters + Set to > 0 to activate a geofence action if vertical distance to home exceeds this value. + -1 + meters + + + + + Consider mount operation mode + If set to 1, mount mode will be enforced. + 0 + 1 + + + Auxilary switch to set mount operation mode + Set to 0 to disable manual mode control. If set to an auxilary switch: Switch off means the gimbal is put into safe/locked position. Switch on means the gimbal can move freely, and landing gear will be retracted if applicable. + 0 + 3 + + + + + L1 period + This is the L1 distance and defines the tracking point ahead of the aircraft its following. A value of 25 meters works for most aircraft. Shorten slowly during tuning until response is sharp without oscillation. + 1.0 + 100.0 + + + L1 damping + Damping factor for L1 control. + 0.6 + 0.9 + + + Cruise throttle + This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7. + 0.0 + 1.0 + + + Throttle max slew rate + Maximum slew rate for the commanded throttle + 0.0 + 1.0 + + + Negative pitch limit + The minimum negative pitch the controller will output. + -60.0 + 0.0 + degrees + + + Positive pitch limit + The maximum positive pitch the controller will output. + 0.0 + 60.0 + degrees + + + Controller roll limit + The maximum roll the controller will output. + 35.0 + 65.0 + degrees + + + Throttle limit max + This is the maximum throttle % that can be used by the controller. For overpowered aircraft, this should be reduced to a value that provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. + + + Throttle limit min + This is the minimum throttle % that can be used by the controller. For electric aircraft this will normally be set to zero, but can be set to a small non-zero value if a folding prop is fitted to prevent the prop from folding and unfolding repeatedly in-flight or to provide some aerodynamic drag from a turning prop to improve the descent rate. For aircraft with internal combustion engine this parameter should be set for desired idle rpm. + 0.0 + 1.0 + + + Throttle limit value before flare + This throttle value will be set as throttle limit at FW_LND_TLALT, before arcraft will flare. + 0.0 + 1.0 + + + Climbout Altitude difference + If the altitude error exceeds this parameter, the system will climb out with maximum throttle and minimum airspeed until it is closer than this distance to the desired altitude. Mostly used for takeoff waypoints / modes. Set to zero to disable climbout mode (not recommended). + 0.0 + 150.0 + + + Maximum climb rate + This is the best climb rate that the aircraft can achieve with the throttle set to THR_MAX and the airspeed set to the default value. For electric aircraft make sure this number can be achieved towards the end of flight when the battery voltage has reduced. The setting of this parameter can be checked by commanding a positive altitude change of 100m in loiter, RTL or guided mode. If the throttle required to climb is close to THR_MAX and the aircraft is maintaining airspeed, then this parameter is set correctly. If the airspeed starts to reduce, then the parameter is set to high, and if the throttle demand required to climb and maintain speed is noticeably less than FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or FW_THR_MAX reduced. + 2.0 + 10.0 + + + Landing slope angle + + + FW_LND_HVIRT + + + Landing flare altitude (relative to landing altitude) + meter + + + Landing throttle limit altitude (relative landing altitude) + Default of -1.0f lets the system default to applying throttle limiting at 2/3 of the flare altitude. + meter + + + Landing heading hold horizontal distance + + + Enable or disable usage of terrain estimate during landing + 0: disabled, 1: enabled + + + Takeoff and landing airspeed scale factor + Multiplying this factor with the minimum airspeed of the plane gives the target airspeed for takeoff and landing approach. + 1.0 + 1.5 + + + + + Multicopter max climb rate + Maximum vertical velocity allowed in the landed state (m/s up and down) + m/s + + + Multicopter max horizontal velocity + Maximum horizontal velocity allowed in the landed state (m/s) + m/s + + + Multicopter max rotation + Maximum allowed around each axis allowed in the landed state (degrees per second) + deg/s + + + Multicopter max throttle + Maximum actuator output on throttle allowed in the landed state + 0.1 + 0.5 + + + Fixedwing max horizontal velocity + Maximum horizontal velocity allowed in the landed state (m/s) + 0.5 + 10 + m/s + + + Fixedwing max climb rate + Maximum vertical velocity allowed in the landed state (m/s up and down) + 5 + 20 + m/s + + + Fixedwing max short-term velocity + Maximum velocity integral in flight direction allowed in the landed state (m/s) + 2 + 10 + m/s + + + Airspeed max + Maximum airspeed allowed in the landed state (m/s) + 4 + 20 + m/s + + + + + Enable launch detection + 0 + 1 + + + Catapult accelerometer theshold + LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + 0 + + + Catapult time theshold + LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + 0 + + + Motor delay + Delay between starting attitude control and powering up the throttle (giving throttle control to the controller) Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate + 0 + seconds + + + Maximum pitch before the throttle is powered up (during motor delay phase) + This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on. This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep). + 0 + 45 + deg + + + Throttle setting while detecting launch + The throttle is set to this value while the system is waiting for the take-off. + 0 + 1 + + + + + Enable local position estimator + + + Enable accelerometer integration for prediction + + + Optical flow xy standard deviation + 0.01 + 1 + m + + + Sonar z standard deviation + 0.01 + 1 + m + + + Lidar z standard deviation + 0.01 + 1 + m + + + Accelerometer xy standard deviation + Data sheet sqrt(Noise power) = 150ug/sqrt(Hz) std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2 Since accels sampled at 1000 Hz. should be 0.0464 + 0.00001 + 2 + m/s^2 + + + Accelerometer z standard deviation + (see Accel x comments) + 0.00001 + 2 + m/s^2 + + + Barometric presssure altitude z standard deviation + 0.01 + 3 + m + + + GPS xy standard deviation + 0.01 + 5 + m + + + GPS z standard deviation + 0.01 + 20 + m + + + GPS xy velocity standard deviation + 0.01 + 2 + m/s + + + GPS z velocity standard deviation + 0.01 + 2 + m/s + + + GPS max eph + 1.0 + 5.0 + m + + + Vision xy standard deviation + 0.01 + 1 + m + + + Vision z standard deviation + 0.01 + 2 + m + + + Circuit breaker to disable vision input + Set to the appropriate key (328754) to disable vision input. + 0 + 1 + + + Vicon position standard deviation + 0.01 + 1 + m + + + Position propagation process noise power (variance*sampling rate) + 0 + 1 + (m/s^2)-s + + + Velocity propagation process noise power (variance*sampling rate) + 0 + 5 + (m/s)-s + + + Accel bias propagation process noise power (variance*sampling rate) + 0 + 1 + (m/s)-s + + + Fault detection threshold, for chi-squared dist + TODO add separate params for 1 dof, 3 dof, and 6 dof beta or false alarm rate in false alarms/hr + 3 + 1000 + + + + + + MAVLink system ID + 1 + 250 + + + MAVLink component ID + 1 + 250 + + + MAVLink Radio ID + When non-zero the MAVLink app will attempt to configure the radio to this ID and re-set the parameter to 0. If the value is negative it will reset the complete radio config to factory defaults. + -1 + 240 + + + MAVLink airframe type + 1 + + + Use/Accept HIL GPS message even if not in HIL mode + If set to 1 incoming HIL GPS messages are parsed. + + + Forward external setpoint messages + If set to 1 incoming external setpoint messages will be directly forwarded to the controllers if in offboard control mode + + + Test parameter + This parameter is not actively used by the system. Its purpose is to allow testing the parameter interface on the communication level. + -1000 + 1000 + + + + + Enables testmode (Identify) of MKBLCTRL Driver + + + + + Take-off altitude + Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to MIS_TAKEOFF_ALT on takeoff, then go to waypoint. + meters + + + Enable persistent onboard mission storage + When enabled, missions that have been uploaded by the GCS are stored and reloaded after reboot persistently. + 0 + 1 + + + Maximal horizontal distance from home to first waypoint + Failsafe check to prevent running mission stored from previous flight at a new takeoff location. Set a value of zero or less to disable. The mission will not be started if the current waypoint is more distant than MIS_DIS_1WP from the current position. + 0 + 1000 + + + Altitude setpoint mode + 0: the system will follow a zero order hold altitude setpoint 1: the system will follow a first order hold altitude setpoint values follow the definition in enum mission_altitude_mode + 0 + 1 + + + Multirotor only. Yaw setpoint mode + 0: Set the yaw heading to the yaw value specified for the destination waypoint. 1: Maintain a yaw heading pointing towards the next waypoint. 2: Maintain a yaw heading that always points to the home location. 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home). The values are defined in the enum mission_altitude_mode + 0 + 3 + + + Loiter radius (FW only) + Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). + 25 + 1000 + meter + + + Acceptance Radius + Default acceptance radius, overridden by acceptance radius of waypoint if set. + 0.05 + 200.0 + meter + + + Set OBC mode for data link loss + If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + 0 + 1 + + + Set OBC mode for rc loss + If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + 0 + 1 + + + + + Max manual roll + 0.0 + 90.0 + deg + + + Max manual pitch + 0.0 + 90.0 + deg + + + Max manual yaw rate + 0.0 + deg/s + + + Roll P gain + Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + + + Roll rate P gain + Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Roll rate I gain + Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Roll rate D gain + Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Pitch P gain + Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + 1/s + + + Pitch rate P gain + Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Pitch rate I gain + Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Pitch rate D gain + Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Yaw P gain + Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + 1/s + + + Yaw rate P gain + Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Yaw rate I gain + Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Yaw rate D gain + Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Yaw feed forward + Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Max yaw rate + Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + 0.0 + 360.0 + deg/s + + + Max acro roll rate + 0.0 + 360.0 + deg/s + + + Max acro pitch rate + 0.0 + 360.0 + deg/s + + + Max acro yaw rate + 0.0 + deg/s + + + Roll P gain + Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + + + Roll rate P gain + Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Roll rate I gain + Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Roll rate D gain + Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Roll rate feedforward + Improves tracking performance. + 0.0 + + + Pitch P gain + Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + 1/s + + + Pitch rate P gain + Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Pitch rate I gain + Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Pitch rate D gain + Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Pitch rate feedforward + Improves tracking performance. + 0.0 + + + Yaw P gain + Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + 1/s + + + Yaw rate P gain + Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Yaw rate I gain + Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Yaw rate D gain + Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Yaw rate feedforward + Improves tracking performance. + 0.0 + + + Yaw feed forward + Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Max roll rate + Limit for roll rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + 0.0 + 360.0 + deg/s + + + Max pitch rate + Limit for pitch rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + 0.0 + 360.0 + deg/s + + + Max yaw rate + Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. A value of significantly over 60 degrees per second can already lead to mixer saturation. + 0.0 + 360.0 + deg/s + + + Max acro roll rate + 0.0 + 360.0 + deg/s + + + Max acro pitch rate + 0.0 + 360.0 + deg/s + + + Max acro yaw rate + 0.0 + deg/s + + + Threshold for Rattitude mode + Manual input needed in order to override attitude control rate setpoints and instead pass manual stick inputs as rate setpoints + 0.0 + 1.0 + + + + + + Minimum thrust + Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + 0.0 + 1.0 + + + Maximum thrust + Limit max allowed thrust. + 0.0 + 1.0 + + + Proportional gain for vertical position error + 0.0 + + + Proportional gain for vertical velocity error + 0.0 + + + Integral gain for vertical velocity error + Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + 0.0 + + + Differential gain for vertical velocity error + 0.0 + + + Maximum vertical velocity + Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL). + 0.0 + m/s + + + Vertical velocity feed forward + Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Proportional gain for horizontal position error + 0.0 + + + Proportional gain for horizontal velocity error + 0.0 + + + Integral gain for horizontal velocity error + Non-zero value allows to resist wind. + 0.0 + + + Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again + 0.0 + + + Maximum horizontal velocity + Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). + 0.0 + m/s + + + Horizontal velocity feed forward + Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Maximum tilt angle in air + Limits maximum tilt in AUTO and POSCTRL modes during flight. + 0.0 + 90.0 + deg + + + Maximum tilt during landing + Limits maximum tilt angle on landing. + 0.0 + 90.0 + deg + + + Landing descend rate + 0.0 + m/s + + + Minimum thrust in auto thrust control + It's recommended to set it > 0 to avoid free fall with zero thrust. + 0.05 + 1.0 + + + Maximum thrust in auto thrust control + Limit max allowed thrust. Setting a value of one can put the system into actuator saturation as no spread between the motors is possible any more. A value of 0.8 - 0.9 is recommended. + 0.0 + 0.95 + + + Minimum manual thrust + Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + 0.0 + 1.0 + + + Maximum manual thrust + Limit max allowed thrust. Setting a value of one can put the system into actuator saturation as no spread between the motors is possible any more. A value of 0.8 - 0.9 is recommended. + 0.0 + 1.0 + + + Proportional gain for vertical position error + 0.0 + + + Proportional gain for vertical velocity error + 0.0 + + + Integral gain for vertical velocity error + Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + 0.0 + + + Differential gain for vertical velocity error + 0.0 + + + Maximum vertical velocity + Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). + 0.0 + 8.0 + m/s + + + Vertical velocity feed forward + Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Proportional gain for horizontal position error + 0.0 + + + Proportional gain for horizontal velocity error + 0.0 + + + Integral gain for horizontal velocity error + Non-zero value allows to resist wind. + 0.0 + + + Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again + 0.0 + + + Maximum horizontal velocity + Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). + 0.0 + m/s + + + Horizontal velocity feed forward + Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Maximum tilt angle in air + Limits maximum tilt in AUTO and POSCTRL modes during flight. + 0.0 + 90.0 + degree + + + Maximum tilt during landing + Limits maximum tilt angle on landing. + 0.0 + 90.0 + degree + + + Landing descend rate + 0.0 + m/s + + + Max manual roll + 0.0 + 90.0 + degree + + + Max manual pitch + 0.0 + 90.0 + degree + + + Max manual yaw rate + 0.0 + degree / s + + + Deadzone of X,Y sticks where position hold is enabled + 0.0 + 1.0 + % + + + Deadzone of Z stick where altitude hold is enabled + 0.0 + 1.0 + % + + + Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) + 0.0 + m/s + + + Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + 0.0 + m/s + + + Low pass filter cut freq. for numerical velocity derivative + 0.0 + Hz + + + Maximum horizonal acceleration in velocity controlled modes + 2.0 + 10.0 + m/s/s + + + + + Set the minimum PWM for the MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. Set to 1000 for industry default or 900 to increase servo travel. + 800 + 1400 + microseconds + + + Set the maximum PWM for the MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. Set to 2000 for industry default or 2100 to increase servo travel. + 1600 + 2200 + microseconds + + + Set the disarmed PWM for MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. This is the PWM pulse the autopilot is outputting if not armed. The main use of this parameter is to silence ESCs when they are disarmed. + 0 + 2200 + microseconds + + + Set the minimum PWM for the MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. Set to 1000 for default or 900 to increase servo travel + 800 + 1400 + microseconds + + + Set the maximum PWM for the MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. Set to 2000 for default or 2100 to increase servo travel + 1600 + 2200 + microseconds + + + Set the disarmed PWM for AUX outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. This is the PWM pulse the autopilot is outputting if not armed. The main use of this parameter is to silence ESCs when they are disarmed. + 0 + 2200 + microseconds + + + Invert direction of main output channel 1 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 2 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 3 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 4 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 5 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 6 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 7 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 8 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Enable S.BUS out + Set to 1 to enable S.BUS version 1 output instead of RSSI. + 0 + 1 + + + Invert direction of aux output channel 1 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 2 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 3 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 4 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 5 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 6 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + + + Ground drag property + This parameter encodes the ground drag coefficient and the corresponding decrease in wind speed from the plane altitude to ground altitude. + 0.001 + 0.1 + unknown + + + Plane turn radius + The planes known minimal turn radius - use a higher value to make the plane maneuver more distant from the actual drop position. This is to ensure the wings are level during the drop. + 30.0 + 500.0 + meter + + + Drop precision + If the system is closer than this distance on passing over the drop position, it will release the payload. This is a safeguard to prevent a drop out of the required accuracy. + 1.0 + 80.0 + meter + + + Payload drag coefficient of the dropped object + The drag coefficient (cd) is the typical drag constant for air. It is in general object specific, but the closest primitive shape to the actual object should give good results: http://en.wikipedia.org/wiki/Drag_coefficient + 0.08 + 1.5 + meter + + + Payload mass + A typical small toy ball: 0.025 kg OBC water bottle: 0.6 kg + 0.001 + 5.0 + kilogram + + + Payload front surface area + A typical small toy ball: (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2 OBC water bottle: (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2 + 0.001 + 0.5 + m^2 + + + + + Velocity estimate delay + The delay in milliseconds of the velocity estimate from GPS. + 0 + 1000 + + + Position estimate delay + The delay in milliseconds of the position estimate from GPS. + 0 + 1000 + + + Height estimate delay + The delay in milliseconds of the height estimate from the barometer. + 0 + 1000 + + + Mag estimate delay + The delay in milliseconds of the magnetic field estimate from the magnetometer. + 0 + 1000 + + + True airspeeed estimate delay + The delay in milliseconds of the airspeed estimate. + 0 + 1000 + + + GPS vs. barometric altitude update weight + RE-CHECK this. + 0.0 + 1.0 + + + Airspeed measurement noise + Increasing this value will make the filter trust this sensor less and trust other sensors more. + 0.5 + 5.0 + + + Velocity measurement noise in north-east (horizontal) direction + Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 + 0.05 + 5.0 + + + Velocity noise in down (vertical) direction + Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7 + 0.2 + 3.0 + + + Position noise in north-east (horizontal) direction + Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5 + 0.1 + 10.0 + + + Position noise in down (vertical) direction + Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0 + 0.5 + 3.0 + + + Magnetometer measurement noise + Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 + 0.01 + 1.0 + + + Gyro process noise + Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015. This noise controls how much the filter trusts the gyro measurements. Increasing it makes the filter trust the gyro less and other sensors more. + 0.001 + 0.05 + + + Accelerometer process noise + Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25. Increasing this value makes the filter trust the accelerometer less and other sensors more. + 0.05 + 1.0 + + + Gyro bias estimate process noise + Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. Increasing this value will make the gyro bias converge faster but noisier. + 0.00000005 + 0.00001 + + + Accelerometer bias estimate process noise + Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f. Increasing this value makes the bias estimation faster and noisier. + 0.00001 + 0.001 + + + Magnetometer earth frame offsets process noise + Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001. Increasing this value makes the magnetometer earth bias estimate converge faster but also noisier. + 0.0001 + 0.01 + + + Magnetometer body frame offsets process noise + Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003. Increasing this value makes the magnetometer body bias estimate converge faster but also noisier. + 0.0001 + 0.01 + + + Magnetometer X bias + The magnetometer bias. This bias is learnt by the filter over time and persists between boots. + -0.6 + 0.6 + + + Magnetometer Y bias + The magnetometer bias. This bias is learnt by the filter over time and persists between boots. + -0.6 + 0.6 + + + Magnetometer Z bias + The magnetometer bias. This bias is learnt by the filter over time and persists between boots. + -0.6 + 0.6 + + + Threshold for filter initialization + If the standard deviation of the GPS position estimate is below this threshold in meters, the filter will initialize. + 0.3 + 10.0 + + + + + Z axis weight for barometer + Weight (cutoff frequency) for barometer altitude measurements. + 0.0 + 10.0 + + + Z axis weight for GPS + Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. + 0.0 + 10.0 + + + Z velocity weight for GPS + Weight (cutoff frequency) for GPS altitude velocity measurements. + 0.0 + 10.0 + + + Z axis weight for vision + Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset. + 0.0 + 10.0 + + + Z axis weight for lidar + Weight (cutoff frequency) for lidar measurements. + 0.0 + 10.0 + + + XY axis weight for GPS position + Weight (cutoff frequency) for GPS position measurements. + 0.0 + 10.0 + + + XY axis weight for GPS velocity + Weight (cutoff frequency) for GPS velocity measurements. + 0.0 + 10.0 + + + XY axis weight for vision position + Weight (cutoff frequency) for vision position measurements. + 0.0 + 10.0 + + + XY axis weight for vision velocity + Weight (cutoff frequency) for vision velocity measurements. + 0.0 + 10.0 + + + Weight for mocap system + Weight (cutoff frequency) for mocap position measurements. + 0.0 + 10.0 + + + XY axis weight for optical flow + Weight (cutoff frequency) for optical flow (velocity) measurements. + 0.0 + 10.0 + + + XY axis weight for resetting velocity + When velocity sources lost slowly decrease estimated horizontal velocity with this weight. + 0.0 + 10.0 + + + XY axis weight factor for GPS when optical flow available + When optical flow data available, multiply GPS weights (for position and velocity) by this factor. + 0.0 + 1.0 + + + Accelerometer bias estimation weight + Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + 0.0 + 0.1 + + + Optical flow scale factor + Factor to convert raw optical flow (in pixels) to radians [rad/px]. + 0.0 + 1.0 + rad/px + + + Minimal acceptable optical flow quality + 0 - lowest quality, 1 - best quality. + 0.0 + 1.0 + + + Weight for lidar filter + Lidar filter detects spikes on lidar measurements and used to detect new surface level. + 0.0 + 1.0 + + + Sonar maximal error for new surface + If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). + 0.0 + 1.0 + m + + + Land detector time + Vehicle assumed landed if no altitude changes happened during this time on low throttle. + 0.0 + 10.0 + s + + + Land detector altitude dispersion threshold + Dispersion threshold for triggering land detector. + 0.0 + 10.0 + m + + + Land detector throttle threshold + Value should be lower than minimal hovering thrust. Half of it is good choice. + 0.0 + 1.0 + + + GPS delay + GPS delay compensation + 0.0 + 1.0 + s + + + Flow module offset (center of rotation) in X direction + Yaw X flow compensation + -1.0 + 1.0 + m + + + Flow module offset (center of rotation) in Y direction + Yaw Y flow compensation + -1.0 + 1.0 + m + + + Disable mocap (set 0 if using fake gps) + Disable mocap + 0 + 1 + + + Disable vision input + Set to the appropriate key (328754) to disable vision input. + 0 + 1 + + + INAV enabled + If set to 1, use INAV for position estimation. Else the system uses the combined attitude / position filter framework. + 0 + 1 + + + + + RC Channel 1 Minimum + Minimum value for RC channel 1 + 800.0 + 1500.0 + us + + + RC Channel 1 Trim + Mid point value (same as min for throttle) + 800.0 + 2200.0 + us + + + RC Channel 1 Maximum + Maximum value for RC channel 1 + 1500.0 + 2200.0 + us + + + RC Channel 1 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 1 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + us + + + RC Channel 2 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 2 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 2 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 2 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 2 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + us + + + RC Channel 3 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 3 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 3 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 3 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 3 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + us + + + RC Channel 4 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 4 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 4 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 4 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 4 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + us + + + RC Channel 5 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 5 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 5 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 5 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 5 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 6 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + + + RC Channel 6 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 6 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 6 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 6 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 7 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + + + RC Channel 7 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 7 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 7 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 7 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 8 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 8 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 8 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 8 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 8 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 9 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 9 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 9 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 9 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 9 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 10 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 10 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 10 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 10 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 10 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 11 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 11 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 11 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 11 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 11 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 12 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 12 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 12 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 12 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 12 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 13 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 13 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 13 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 13 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 13 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 14 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 14 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 14 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 14 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 14 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 15 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 15 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 15 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 15 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 15 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 16 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 16 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 16 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 16 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 16 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 17 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 17 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 17 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 17 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 17 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 18 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 18 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 18 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 18 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 18 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + Enable relay control of relay 1 mapped to the Spektrum receiver power supply + 0 + 1 + + + DSM binding trigger + -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind + + + RC channel count + This parameter is used by Ground Station software to save the number of channels which were used during RC calibration. It is only meant for ground station use. + 0 + 18 + + + RC mode switch threshold automaic distribution + This parameter is used by Ground Station software to specify whether the threshold values for flight mode switches were automatically calculated. 0 indicates that the threshold values were set by the user. Any other value indicates that the threshold value where automatically set by the ground station software. It is only meant for ground station use. + 0 + 1 + + + Roll control channel mapping + The channel index (starting from 1 for channel 1) indicates which channel should be used for reading roll inputs from. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Pitch control channel mapping + The channel index (starting from 1 for channel 1) indicates which channel should be used for reading pitch inputs from. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Throttle control channel mapping + The channel index (starting from 1 for channel 1) indicates which channel should be used for reading throttle inputs from. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Yaw control channel mapping + The channel index (starting from 1 for channel 1) indicates which channel should be used for reading yaw inputs from. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Auxiliary switch 1 channel mapping + Default function: Camera pitch + 0 + 18 + + + Auxiliary switch 2 channel mapping + Default function: Camera roll + 0 + 18 + + + Auxiliary switch 3 channel mapping + Default function: Camera azimuth / yaw + 0 + 18 + + + Channel which changes a parameter + Can be used for parameter tuning with the RC. This one is further referenced as the 1st parameter channel. Set to 0 to deactivate * + 0 + 18 + + + Channel which changes a parameter + Can be used for parameter tuning with the RC. This one is further referenced as the 2nd parameter channel. Set to 0 to deactivate * + 0 + 18 + + + Channel which changes a parameter + Can be used for parameter tuning with the RC. This one is further referenced as the 3th parameter channel. Set to 0 to deactivate * + 0 + 18 + + + Failsafe channel PWM threshold + Set to a value slightly above the PWM value assumed by throttle in a failsafe event, but ensure it is below the PWM value assumed by throttle during normal operation. + 0 + 2200 + us + + + PWM input channel that provides RSSI + 0: do not read RSSI from input channel 1-18: read RSSI from specified input channel Specify the range for RSSI input with RC_RSSI_PWM_MIN and RC_RSSI_PWM_MAX parameters. + 0 + 18 + + + Max input value for RSSI reading + Only used if RC_RSSI_PWM_CHAN > 0 + 0 + 2000 + + + Min input value for RSSI reading + Only used if RC_RSSI_PWM_CHAN > 0 + 0 + 2000 + + + Roll trim + The trim value is the actuator control value the system needs for straight and level flight. It can be calibrated by flying manually straight and level using the RC trims and copying them using the GCS. + -0.25 + 0.25 + 2 + + + Pitch trim + The trim value is the actuator control value the system needs for straight and level flight. It can be calibrated by flying manually straight and level using the RC trims and copying them using the GCS. + -0.25 + 0.25 + 2 + + + Yaw trim + The trim value is the actuator control value the system needs for straight and level flight. It can be calibrated by flying manually straight and level using the RC trims and copying them using the GCS. + -0.25 + 0.25 + 2 + + + + + Loiter Time + The amount of time in seconds the system should loiter at current position before termination Set to -1 to make the system skip loitering + -1.0 + seconds + + + + + Mode switch channel mapping + This is the main flight mode selector. The channel index (starting from 1 for channel 1) indicates which channel should be used for deciding about the main mode. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Return switch channel mapping + 0 + 18 + + + Rattitude switch channel mapping + 0 + 18 + + + Posctl switch channel mapping + 0 + 18 + + + Loiter switch channel mapping + 0 + 18 + + + Acro switch channel mapping + 0 + 18 + + + Offboard switch channel mapping + 0 + 18 + + + Flaps channel mapping + 0 + 18 + + + Threshold for selecting assist mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting auto mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting rattitude mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting posctl mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting return to launch mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting loiter mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting acro mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting offboard mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + + + RTL altitude + Altitude to fly back in RTL in meters + 0 + 150 + meters + + + RTL loiter altitude + Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. + 2 + 100 + meters + + + RTL delay + Delay after descend before landing in RTL mode. If set to -1 the system will not land but loiter at NAV_LAND_ALT. + -1 + 300 + seconds + + + + + Enable or disable runway takeoff with landing gear + 0: disabled, 1: enabled + 0 + 1 + + + Specifies which heading should be held during runnway takeoff + 0: airframe heading, 1: heading towards takeoff waypoint + 0 + 1 + + + Altitude AGL at which we have enough ground clearance to allow some roll. + Until RWTO_NAV_ALT is reached the plane is held level and only + rudder is used to keep the heading (see RWTO_HDG). This should be below + FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0 + 0.0 + 100.0 + m + + + Max throttle during runway takeoff. + (Can be used to test taxi on runway) + 0.0 + 1.0 + + + Pitch setpoint during taxi / before takeoff airspeed is reached. + A taildragger with stearable wheel might need to pitch up + a little to keep it's wheel on the ground before airspeed + to takeoff is reached + 0.0 + 20.0 + deg + + + Max pitch during takeoff. + Fixed-wing settings are used if set to 0. Note that there is also a minimum + pitch of 10 degrees during takeoff, so this must be larger if set + 0.0 + 60.0 + deg + + + Max roll during climbout. + Roll is limited during climbout to ensure enough lift and prevents aggressive + navigation before we're on a safe height + 0.0 + 60.0 + deg + + + Min. airspeed scaling factor for takeoff. + Pitch up will be commanded when the following airspeed is reached: + FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + 0.0 + 2.0 + + + + + Logging rate + A value of -1 indicates the commandline argument should be obeyed. A value of 0 sets the minimum rate, any other value is interpreted as rate in Hertz. This parameter is only read out before logging starts (which commonly is before arming). + -1 + 100 + + + Enable extended logging mode + A value of -1 indicates the commandline argument should be obeyed. A value of 0 disables extended logging mode, a value of 1 enables it. This parameter is only read out before logging starts (which commonly is before arming). + -1 + 1 + + + Use timestamps only if GPS 3D fix is available + A value of 1 constrains the log folder creation to only use the time stamp if a 3D GPS lock is present. + 0 + 1 + + + + + ID of the board this parameter set was calibrated on + + + ID of the Gyro that the calibration is for + + + Gyro X-axis offset + -10.0 + 10.0 + + + Gyro Y-axis offset + -10.0 + 10.0 + + + Gyro Z-axis offset + -5.0 + 5.0 + + + Gyro X-axis scaling factor + -1.5 + 1.5 + + + Gyro Y-axis scaling factor + -1.5 + 1.5 + + + Gyro Z-axis scaling factor + -1.5 + 1.5 + + + ID of Magnetometer the calibration is for + + + Rotation of magnetometer 0 relative to airframe + An internal magnetometer will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + -1 + 30 + + + Magnetometer X-axis offset + -500.0 + 500.0 + + + Magnetometer Y-axis offset + -500.0 + 500.0 + + + Magnetometer Z-axis offset + -500.0 + 500.0 + + + Magnetometer X-axis scaling factor + + + Magnetometer Y-axis scaling factor + + + Magnetometer Z-axis scaling factor + + + ID of the Accelerometer that the calibration is for + + + Accelerometer X-axis offset + + + Accelerometer Y-axis offset + + + Accelerometer Z-axis offset + + + Accelerometer X-axis scaling factor + + + Accelerometer Y-axis scaling factor + + + Accelerometer Z-axis scaling factor + + + ID of the Gyro that the calibration is for + + + Gyro X-axis offset + -10.0 + 10.0 + + + Gyro Y-axis offset + -10.0 + 10.0 + + + Gyro Z-axis offset + -5.0 + 5.0 + + + Gyro X-axis scaling factor + -1.5 + 1.5 + + + Gyro Y-axis scaling factor + -1.5 + 1.5 + + + Gyro Z-axis scaling factor + -1.5 + 1.5 + + + ID of Magnetometer the calibration is for + + + Rotation of magnetometer 1 relative to airframe + An internal magnetometer will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + -1 + 30 + + + Magnetometer X-axis offset + -500.0 + 500.0 + + + Magnetometer Y-axis offset + -500.0 + 500.0 + + + Magnetometer Z-axis offset + -500.0 + 500.0 + + + Magnetometer X-axis scaling factor + + + Magnetometer Y-axis scaling factor + + + Magnetometer Z-axis scaling factor + + + ID of the Accelerometer that the calibration is for + + + Accelerometer X-axis offset + + + Accelerometer Y-axis offset + + + Accelerometer Z-axis offset + + + Accelerometer X-axis scaling factor + + + Accelerometer Y-axis scaling factor + + + Accelerometer Z-axis scaling factor + + + ID of the Gyro that the calibration is for + + + Gyro X-axis offset + -10.0 + 10.0 + + + Gyro Y-axis offset + -10.0 + 10.0 + + + Gyro Z-axis offset + -5.0 + 5.0 + + + Gyro X-axis scaling factor + -1.5 + 1.5 + + + Gyro Y-axis scaling factor + -1.5 + 1.5 + + + Gyro Z-axis scaling factor + -1.5 + 1.5 + + + ID of Magnetometer the calibration is for + + + Rotation of magnetometer 2 relative to airframe + An internal magnetometer will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + -1 + 30 + + + Magnetometer X-axis offset + -500.0 + 500.0 + + + Magnetometer Y-axis offset + -500.0 + 500.0 + + + Magnetometer Z-axis offset + -500.0 + 500.0 + + + Magnetometer X-axis scaling factor + + + Magnetometer Y-axis scaling factor + + + Magnetometer Z-axis scaling factor + + + ID of the Accelerometer that the calibration is for + + + Accelerometer X-axis offset + + + Accelerometer Y-axis offset + + + Accelerometer Z-axis offset + + + Accelerometer X-axis scaling factor + + + Accelerometer Y-axis scaling factor + + + Accelerometer Z-axis scaling factor + + + Primary accel ID + + + Primary gyro ID + + + Primary mag ID + + + Primary baro ID + + + Differential pressure sensor offset + The offset (zero-reading) in Pascal + + + Differential pressure sensor analog scaling + Pick the appropriate scaling from the datasheet. this number defines the (linear) conversion from voltage to Pascal (pa). For the MPXV7002DP this is 1000. NOTE: If the sensor always registers zero, try switching the static and dynamic tubes. + + + QNH for barometer + 500 + 1500 + hPa + + + Board rotation + This parameter defines the rotation of the FMU board relative to the platform. Possible values are: 0 = No rotation 1 = Yaw 45° 2 = Yaw 90° 3 = Yaw 135° 4 = Yaw 180° 5 = Yaw 225° 6 = Yaw 270° 7 = Yaw 315° 8 = Roll 180° 9 = Roll 180°, Yaw 45° 10 = Roll 180°, Yaw 90° 11 = Roll 180°, Yaw 135° 12 = Pitch 180° 13 = Roll 180°, Yaw 225° 14 = Roll 180°, Yaw 270° 15 = Roll 180°, Yaw 315° 16 = Roll 90° 17 = Roll 90°, Yaw 45° 18 = Roll 90°, Yaw 90° 19 = Roll 90°, Yaw 135° 20 = Roll 270° 21 = Roll 270°, Yaw 45° 22 = Roll 270°, Yaw 90° 23 = Roll 270°, Yaw 135° 24 = Pitch 90° 25 = Pitch 270° + + + PX4Flow board rotation + This parameter defines the rotation of the PX4FLOW board relative to the platform. Zero rotation is defined as Y on flow board pointing towards front of vehicle Possible values are: 0 = No rotation 1 = Yaw 45° 2 = Yaw 90° 3 = Yaw 135° 4 = Yaw 180° 5 = Yaw 225° 6 = Yaw 270° 7 = Yaw 315° + + + Board rotation Y (Pitch) offset + This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user to fine tune the board offset in the event of misalignment. + degrees + + + Board rotation X (Roll) offset + This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user to fine tune the board offset in the event of misalignment. + degrees + + + Board rotation Z (YAW) offset + This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user to fine tune the board offset in the event of misalignment. + degrees + + + External magnetometer rotation + This parameter defines the rotation of the external magnetometer relative to the platform (not relative to the FMU). See SENS_BOARD_ROT for possible values. + + + Set usage of external magnetometer + * Set to 0 (default) to auto-detect (will try to get the external as primary) * Set to 1 to force the external magnetometer as primary * Set to 2 to force the internal magnetometer as primary + 0 + 2 + + + + + Enable Lidar-Lite (LL40LS) pwm driver + 0 + 1 + + + + + Interval of one subscriber in the example in ms + + + Float Demonstration Parameter in the Example + + + + + Auto-start script index + CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system. + + + Automatically configure default values + Set to 1 to reset parameters on next system startup (setting defaults). Platform-specific values are used if available. RC* parameters are preserved. + 0 + 1 + + + Set usage of IO board + Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. + 0 + 1 + + + Set restart type + Set by px4io to indicate type of restart + 0 + 2 + + + Companion computer interface + CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the companion computer interface. Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!) 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1. 157600: enables OSD mode at 57600 baud, 8N1. + 0 + 921600 + + + Parameter version + This monotonically increasing number encodes the parameter compatibility set. whenever it increases parameters might not be backwards compatible and ground control stations should suggest a fresh configuration. + 0 + + + + + Enable UAVCAN + Allowed values: 0 - UAVCAN disabled. 1 - Enabled support for UAVCAN actuators and sensors. 2 - Enabled support for dynamic node ID allocation and firmware update. 3 - Sets the motor control outputs to UAVCAN and enables support for dynamic node ID allocation and firmware update. + 0 + 3 + + + UAVCAN Node ID + Read the specs at http://uavcan.org to learn more about Node ID. + 1 + 125 + + + UAVCAN CAN bus bitrate + 20000 + 1000000 + + + + + Position of tilt servo in mc mode + Position of tilt servo in mc mode + 0.0 + 1 + + + Position of tilt servo in transition mode + Position of tilt servo in transition mode + 0.0 + 1 + + + Position of tilt servo in fw mode + Position of tilt servo in fw mode + 0.0 + 1 + + + Duration of front transition phase 2 + Time in seconds it should take for the rotors to rotate forward completely from the point when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + 0.1 + 2 + + + The channel number of motors that must be turned off in fixed wing mode + 0 + 123456 + + + Target throttle value for pusher/puller motor during the transition to fw mode + 0.0 + 1.0 + + + VTOL number of engines + 0 + + + Idle speed of VTOL when in multicopter mode + 900 + + + Minimum airspeed in multicopter mode + This is the minimum speed of the air flowing over the control surfaces. + 0.0 + + + Maximum airspeed in multicopter mode + This is the maximum speed of the air flowing over the control surfaces. + 0.0 + + + Trim airspeed when in multicopter mode + This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode. + 0.0 + + + Permanent stabilization in fw mode + If set to one this parameter will cause permanent attitude stabilization in fw mode. This parameter has been introduced for pure convenience sake. + 0 + 1 + + + Fixed wing pitch trim + This parameter allows to adjust the neutral elevon position in fixed wing mode. + -1 + 1 + + + Motor max power + Indicates the maximum power the motor is able to produce. Used to calculate propeller efficiency map. + 1 + + + Propeller efficiency parameter + Influences propeller efficiency at different power settings. Should be tuned beforehand. + 0.0 + 0.9 + + + Total airspeed estimate low-pass filter gain + Gain for tuning the low-pass filter for the total airspeed estimate + 0.0 + 0.99 + + + VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) + 0 + 2 + + + Lock elevons in multicopter mode + If set to 1 the elevons are locked in multicopter mode + 0 + 1 + + + Duration of a front transition + Time in seconds used for a transition + 0.0 + 5 + + + Duration of a back transition + Time in seconds used for a back transition + 0.0 + 5 + + + Transition blending airspeed + Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable. + 0.0 + 20.0 + + + Transition airspeed + Airspeed at which we can switch to fw mode + 1.0 + 20 + + + + + mTECS enabled + Set to 1 to enable mTECS + 0 + 1 + + + Total Energy Rate Control Feedforward + Maps the total energy rate setpoint to the throttle setpoint + 0.0 + 10.0 + + + Total Energy Rate Control P + Maps the total energy rate error to the throttle setpoint + 0.0 + 10.0 + + + Total Energy Rate Control I + Maps the integrated total energy rate to the throttle setpoint + 0.0 + 10.0 + + + Total Energy Rate Control Offset (Cruise throttle sp) + 0.0 + 10.0 + + + Energy Distribution Rate Control Feedforward + Maps the energy distribution rate setpoint to the pitch setpoint + 0.0 + 10.0 + + + Energy Distribution Rate Control P + Maps the energy distribution rate error to the pitch setpoint + 0.0 + 10.0 + + + Energy Distribution Rate Control I + Maps the integrated energy distribution rate error to the pitch setpoint + 0.0 + 10.0 + + + Total Energy Distribution Offset (Cruise pitch sp) + 0.0 + 10.0 + + + Minimal Throttle Setpoint + 0.0 + 1.0 + + + Maximal Throttle Setpoint + 0.0 + 1.0 + + + Minimal Pitch Setpoint in Degrees + -90.0 + 90.0 + deg + + + Maximal Pitch Setpoint in Degrees + -90.0 + 90.0 + deg + + + Lowpass (cutoff freq.) for altitude + + + Lowpass (cutoff freq.) for the flight path angle + + + P gain for the altitude control + Maps the altitude error to the flight path angle setpoint + 0.0 + 10.0 + + + D gain for the altitude control + Maps the change of altitude error to the flight path angle setpoint + 0.0 + 10.0 + + + Lowpass for FPA error derivative calculation (see MT_FPA_D) + + + Minimal flight path angle setpoint + -90.0 + 90.0 + deg + + + Maximal flight path angle setpoint + -90.0 + 90.0 + deg + + + Lowpass (cutoff freq.) for airspeed + + + Airspeed derivative calculation lowpass + + + P gain for the airspeed control + Maps the airspeed error to the acceleration setpoint + 0.0 + 10.0 + + + D gain for the airspeed control + Maps the change of airspeed error to the acceleration setpoint + 0.0 + 10.0 + + + Lowpass for ACC error derivative calculation (see MT_ACC_D) + + + Minimal acceleration (air) + m/s^2 + + + Maximal acceleration (air) + m/s^2 + + + Minimal throttle during takeoff + 0.0 + 1.0 + + + Maximal throttle during takeoff + 0.0 + 1.0 + + + Minimal pitch during takeoff + -90.0 + 90.0 + deg + + + Maximal pitch during takeoff + -90.0 + 90.0 + deg + + + Minimal throttle in underspeed mode + 0.0 + 1.0 + + + Maximal throttle in underspeed mode + 0.0 + 1.0 + + + Minimal pitch in underspeed mode + -90.0 + 90.0 + deg + + + Maximal pitch in underspeed mode + -90.0 + 90.0 + deg + + + Minimal throttle in landing mode (only last phase of landing) + 0.0 + 1.0 + + + Maximal throttle in landing mode (only last phase of landing) + 0.0 + 1.0 + + + Minimal pitch in landing mode + -90.0 + 90.0 + deg + + + Maximal pitch in landing mode + -90.0 + 90.0 + deg + + + Integrator Limit for Total Energy Rate Control + 0.0 + 10.0 + + + Integrator Limit for Energy Distribution Rate Control + 0.0 + 10.0 + + + + + RV_YAW_P + + + EXFW_HDNG_P + + + EXFW_ROLL_P + + + EXFW_PITCH_P + + + Failsafe channel mapping + The RC mapping index indicates which channel is used for failsafe If 0, whichever channel is mapped to throttle is used otherwise the value indicates the specific rc channel to use + 0 + 18 + + + TEST_MIN + + + TEST_MAX + + + TEST_TRIM + + + TEST_HP + + + TEST_LP + + + TEST_P + + + TEST_I + + + TEST_I_MAX + + + TEST_D + + + TEST_D_LP + + + TEST_MEAN + + + TEST_DEV + + + Flare, minimum pitch + Minimum pitch during flare, a positive sign means nose up Applied once FW_LND_TLALT is reached + + + Flare, maximum pitch + Maximum pitch during flare, a positive sign means nose up Applied once FW_LND_TLALT is reached + + + SEG_TH2V_P + + + SEG_TH2V_I + + + SEG_TH2V_I_MAX + + + SEG_Q2V + + + FWB_P_LP + + + FWB_Q_LP + + + FWB_R_LP + + + FWB_R_HP + + + FWB_P2AIL + + + FWB_Q2ELV + + + FWB_R2RDR + + + FWB_PSI2PHI + + + FWB_PHI2P + + + FWB_PHI_LIM_MAX + + + FWB_V2THE_P + + + FWB_V2THE_I + + + FWB_V2THE_D + + + FWB_V2THE_D_LP + + + FWB_V2THE_I_MAX + + + FWB_THE_MIN + + + FWB_THE_MAX + + + FWB_THE2Q_P + + + FWB_THE2Q_I + + + FWB_THE2Q_D + + + FWB_THE2Q_D_LP + + + FWB_THE2Q_I_MAX + + + FWB_H2THR_P + + + FWB_H2THR_I + + + FWB_H2THR_D + + + FWB_H2THR_D_LP + + + FWB_H2THR_I_MAX + + + FWB_XT2YAW_MAX + + + FWB_XT2YAW + + + FWB_V_MIN + + + FWB_V_CMD + + + FWB_V_MAX + + + FWB_CR_MAX + + + FWB_CR2THR_P + + + FWB_CR2THR_I + + + FWB_CR2THR_D + + + FWB_CR2THR_D_LP + + + FWB_CR2THR_I_MAX + + + FWB_TRIM_THR + + + FWB_TRIM_V + + + RGB Led brightness limit + Set to 0 to disable, 1 for minimum brightness up to 15 (max) + 0 + 15 + + + + \ No newline at end of file diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/px4/Px4Native.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/px4/Px4Native.java index ef9ef5974d..32f0759d82 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/px4/Px4Native.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/px4/Px4Native.java @@ -1,15 +1,21 @@ package org.droidplanner.services.android.core.drone.autopilot.px4; import android.content.Context; +import android.os.Bundle; import android.os.Handler; import com.MAVLink.Messages.MAVLinkMessage; +import com.o3dr.services.android.lib.drone.action.ParameterActions; +import com.o3dr.services.android.lib.model.ICommandListener; +import com.o3dr.services.android.lib.model.action.Action; import org.droidplanner.services.android.communication.model.DataLink; import org.droidplanner.services.android.core.drone.LogMessageListener; import org.droidplanner.services.android.core.drone.autopilot.generic.GenericMavLinkDrone; +import org.droidplanner.services.android.core.drone.variables.HeartBeat; import org.droidplanner.services.android.core.firmware.FirmwareType; import org.droidplanner.services.android.core.model.AutopilotWarningParser; +import org.droidplanner.services.android.utils.CommonApiUtils; /** * Created by Fredia Huya-Kouadio on 9/10/15. @@ -25,4 +31,37 @@ public FirmwareType getFirmwareType() { return FirmwareType.PX4_NATIVE; } + @Override + protected HeartBeat initHeartBeat(Handler handler) { + return new HeartBeat(this, handler); + } + + + @Override + public boolean executeAsyncAction(Action action, ICommandListener listener) { + String type = action.getType(); + Bundle data = action.getData(); + if (data == null) { + data = new Bundle(); + } + switch (type) { + + case ParameterActions.ACTION_REFRESH_PARAMETERS: + CommonApiUtils.refreshParameters(this); + return true; + + default: + return super.executeAsyncAction(action, listener); + } + + } + + @Override + public void onMavLinkMessageReceived(MAVLinkMessage message) { + + getParameterManager().processMessage(message); + + super.onMavLinkMessageReceived(message); + } + } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterManager.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterManager.java index 414fb7aeff..07c08a80bb 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterManager.java @@ -3,11 +3,13 @@ import android.content.Context; import android.os.Handler; import android.text.TextUtils; +import android.util.Log; import android.util.SparseBooleanArray; import com.MAVLink.Messages.MAVLinkMessage; import com.MAVLink.common.msg_param_value; import com.o3dr.services.android.lib.drone.property.Parameter; +import com.o3dr.services.android.lib.drone.property.Type; import org.droidplanner.services.android.core.MAVLink.MavLinkParameters; import org.droidplanner.services.android.core.drone.DroneInterfaces; @@ -15,7 +17,9 @@ import org.droidplanner.services.android.core.drone.DroneInterfaces.OnDroneListener; import org.droidplanner.services.android.core.drone.DroneVariable; import org.droidplanner.services.android.core.drone.autopilot.MavLinkDrone; +import org.droidplanner.services.android.core.firmware.FirmwareType; import org.droidplanner.services.android.utils.file.IO.ParameterMetadataLoader; +import org.droidplanner.services.android.utils.file.IO.ParameterMetadataLoaderPX4; import java.util.Locale; import java.util.Map; @@ -64,12 +68,15 @@ public void run() { private final SparseBooleanArray paramsRollCall = new SparseBooleanArray(); private final ConcurrentHashMap parameters = new ConcurrentHashMap<>(); private final ConcurrentHashMap parametersMetadata = new ConcurrentHashMap<>(); + private final ConcurrentHashMap parametersMetadataPX4 = new ConcurrentHashMap<>(); private DroneInterfaces.OnParameterManagerListener parameterListener; private final Handler watchdog; private final Context context; + private String metadataType; + public ParameterManager(MavLinkDrone myDrone, Context context, Handler handler) { super(myDrone); this.context = context; @@ -84,7 +91,7 @@ public void refreshParameters() { parameters.clear(); paramsRollCall.clear(); - notifyParametersReceiptStart(); + notifyParametersReceiptStart(); // Tower --> LocalBroadcast --> PARAMETERS_REFRESH_STARTED MavLinkParameters.requestParametersList(myDrone); resetWatchdog(); @@ -108,8 +115,11 @@ public Map getParameters() { */ public boolean processMessage(MAVLinkMessage msg) { if (msg.msgid == msg_param_value.MAVLINK_MSG_ID_PARAM_VALUE) { + Log.d("kiba", "processMessage: msg.msgid == msg_param_value.MAVLINK_MSG_ID_PARAM_VALUE? " + true); processReceivedParam((msg_param_value) msg); return true; + }else{ + Log.d("kiba", "processMessage: msg.msgid == msg_param_value.MAVLINK_MSG_ID_PARAM_VALUE? " + false); } return false; } @@ -120,12 +130,13 @@ protected void processReceivedParam(msg_param_value m_value) { loadParameterMetadata(param); parameters.put(param.getName().toLowerCase(Locale.US), param); + Log.d("kiba", "processReceivedParam: parameters size = " + parameters.size()); int paramIndex = m_value.param_index; if (paramIndex == -1) { // update listener notifyParameterReceipt(param, 0, 1); - notifyParametersReceiptEnd(); + notifyParametersReceiptEnd(); // Tower --> PARAMETERS_REFRESH_COMPLETED return; } @@ -140,7 +151,7 @@ protected void processReceivedParam(msg_param_value m_value) { killWatchdog(); isRefreshing.set(false); - notifyParametersReceiptEnd(); + notifyParametersReceiptEnd(); //Tower --> PARAMETERS_REFRESH_COMPLETED } else { resetWatchdog(); } @@ -213,14 +224,20 @@ public void onDroneEvent(DroneEventsType event, MavLinkDrone drone) { private void refreshParametersMetadata() { //Reload the vehicle parameters metadata String metadataType = myDrone.getFirmwareType().getParameterMetadataGroup(); + this.metadataType = metadataType; if (!TextUtils.isEmpty(metadataType)) { try { - ParameterMetadataLoader.load(context, metadataType, this.parametersMetadata); + if(metadataType.equals(FirmwareType.PX4_NATIVE.getParameterMetadataGroup())){ + ParameterMetadataLoaderPX4.load(context, metadataType, this.parametersMetadataPX4); + Log.d("kiba", "parseMetadataPX4: size = " + parametersMetadataPX4.size()); + }else{ + ParameterMetadataLoader.load(context, metadataType, this.parametersMetadata); + Log.d("kiba", "parseMetadata: size = " + parametersMetadata.size()); + } } catch (Exception e) { Timber.e(e, e.getMessage()); } } - if (parametersMetadata.isEmpty() || parameters.isEmpty()) return; @@ -230,14 +247,26 @@ private void refreshParametersMetadata() { } private void loadParameterMetadata(Parameter parameter){ - ParameterMetadata metadata = parametersMetadata.get(parameter.getName()); - if (metadata != null) { - parameter.setDisplayName(metadata.getDisplayName()); - parameter.setDescription(metadata.getDescription()); - parameter.setUnits(metadata.getUnits()); - parameter.setRange(metadata.getRange()); - parameter.setValues(metadata.getValues()); + if(metadataType.equals(FirmwareType.PX4_NATIVE.getParameterMetadataGroup())){ // PX4 + ParameterMetadataPX4 metadata = parametersMetadataPX4.get(parameter.getName()); + if (metadata != null) { + parameter.setDisplayName(metadata.getDisplayName()); + parameter.setDescription(metadata.getDescription()); + parameter.setUnits(metadata.getUnits()); + parameter.setRange(metadata.getRange()); + parameter.setValues(metadata.getValues()); + } + }else{ // ArduPilot + ParameterMetadata metadata = parametersMetadata.get(parameter.getName()); + if (metadata != null) { + parameter.setDisplayName(metadata.getDisplayName()); + parameter.setDescription(metadata.getDescription()); + parameter.setUnits(metadata.getUnits()); + parameter.setRange(metadata.getRange()); + parameter.setValues(metadata.getValues()); + } } + } public void setParameterListener(DroneInterfaces.OnParameterManagerListener parameterListener) { diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterMetadataPX4.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterMetadataPX4.java new file mode 100644 index 0000000000..44fbe09f38 --- /dev/null +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterMetadataPX4.java @@ -0,0 +1,75 @@ +package org.droidplanner.services.android.core.drone.profiles; + +/** + * Created by KiBa on 2015/12/21. + */ +public class ParameterMetadataPX4 extends ParameterMetadata{ + + private String decimal; + private String groupName; + private String defaultAttr; + private String typeAttr; + private String max; + private String min; + + public String getDecimal() { + return decimal; + } + + public String getGroupName() { + return groupName; + } + + public String getMax() { + return max; + } + + public String getMin() { + return min; + } + + public String getDefaultAttr() { + return defaultAttr; + } + + public String getTypeAttr() { + return typeAttr; + } + + public void setDecimal(String decimal) { + this.decimal = decimal; + } + + public void setGroupName(String groupName) { + this.groupName = groupName; + } + + public void setDefaultAttr(String defaultAttr) { + this.defaultAttr = defaultAttr; + } + + public void setTypeAttr(String typeAttr) { + this.typeAttr = typeAttr; + } + + public void setMax(String max) { + this.max = max; + } + + public void setMin(String min) { + this.min = min; + } + + public String toString(){ + String str = " group = " + getGroupName() + + "\n name = " + getName() + + "\n default = " + getDefaultAttr() + + "\n type = " + getTypeAttr() + + "\n display = " + getDisplayName() + + "\n description = " + getDescription() + + "\n unit = " + getUnits() + + "\n range = " + getRange() + + "\n values = " + getValues(); + return str; + } +} diff --git a/ServiceApp/src/org/droidplanner/services/android/core/firmware/FirmwareType.java b/ServiceApp/src/org/droidplanner/services/android/core/firmware/FirmwareType.java index 4069d10183..521fa36eb3 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/firmware/FirmwareType.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/firmware/FirmwareType.java @@ -12,7 +12,7 @@ public enum FirmwareType { /** * PX4 firmware type */ - PX4_NATIVE(MAV_AUTOPILOT.MAV_AUTOPILOT_PX4, "", "PX4 Native"), + PX4_NATIVE(MAV_AUTOPILOT.MAV_AUTOPILOT_PX4, "PX4", "PX4 Native"), /** * Generic firmware type diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/file/IO/ParameterMetadataLoaderPX4.java b/ServiceApp/src/org/droidplanner/services/android/utils/file/IO/ParameterMetadataLoaderPX4.java new file mode 100644 index 0000000000..59106ebfb8 --- /dev/null +++ b/ServiceApp/src/org/droidplanner/services/android/utils/file/IO/ParameterMetadataLoaderPX4.java @@ -0,0 +1,205 @@ +package org.droidplanner.services.android.utils.file.IO; + +import android.content.Context; +import android.text.TextUtils; +import android.util.Xml; + +import org.droidplanner.services.android.core.drone.profiles.ParameterMetadataPX4; +import org.xmlpull.v1.XmlPullParser; +import org.xmlpull.v1.XmlPullParserException; + +import java.io.IOException; +import java.io.InputStream; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; + +/** + * Created by KiBa on 2015/12/24. + */ +public class ParameterMetadataLoaderPX4 { + + private static final String PARAMETERMETADATA_PATH = "Parameters/ParameterMetaData.xml"; + + //**********************PX4 metadata************************ + private static final String METADATA_GROUP_PX4 = "group"; + private static final String METADATA_PARAMETER_PX4 = "parameter"; + + private static final String METADATA_DISPLAYNAME_PX4 = "short_desc"; + private static final String METADATA_DESCRIPTION_PX4 = "long_desc"; + private static final String METADATA_UNITS_PX4 = "unit"; + private static final String METADATA_RANGE_MAX_PX4 = "max"; + private static final String METADATA_RANGE_MIN_PX4 = "min"; + private static final String METADATA_DECIMAL_PX4 = "decimal"; + + private static final String METADATA_TYPE_PX4_FLOAT = "FLOAT"; + private static final String METADATA_TYPE_PX4_INT32 = "INT32"; + private static List minListInt32 = new ArrayList<>(); + private static List minListFloat = new ArrayList<>(); + + public static void load(Context context, String metadataType, Map metadata) + throws IOException, XmlPullParserException { + InputStream inputStream = context.getAssets().open(PARAMETERMETADATA_PATH); + open(inputStream, metadataType, metadata); + } + + private static void open(InputStream inputStream, String metadataType, Map metadataMap) + throws XmlPullParserException, IOException { + try { + XmlPullParser parser = Xml.newPullParser(); + parser.setFeature(XmlPullParser.FEATURE_PROCESS_NAMESPACES, false); + parser.setInput(inputStream, null); + parseMetadataPX4(parser, metadataType, metadataMap); + + } finally { + try { + inputStream.close(); + } catch (IOException e) { /* nop */ + } + } + } + + //**************************** PX4 ********************************* + + private static void parseMetadataPX4(XmlPullParser pullParser, String metadataType, + Map metadataMap) throws XmlPullParserException, IOException { + int type = pullParser.getEventType(); + + ParameterMetadataPX4 metadataPX4 = null; + String name = null; // label name + String groupName = null; // group--'name' attribute + String defaultAttr = null; // parameter--'default' attribute + String nameAttr = null; // parameter--'name' attribute + String typeAttr = null; // parameter--'type' attribute + + boolean parsing = false; + + while (type != XmlPullParser.END_DOCUMENT) { + switch (type) { + case XmlPullParser.START_TAG: + name = pullParser.getName(); // label name + + if (TextUtils.equals(metadataType, name)) { + parsing = true; + } else if (parsing) { + // if the label name is + if (TextUtils.equals(pullParser.getName(), METADATA_GROUP_PX4)) { + groupName = pullParser.getAttributeValue(0); + } else if (TextUtils.equals(pullParser.getName(), METADATA_PARAMETER_PX4)) { // if label name isn't 'group' + metadataPX4 = new ParameterMetadataPX4(); + // get attributes of the label + if (groupName != null) { + metadataPX4.setGroupName(groupName); // group value + } + defaultAttr = pullParser.getAttributeValue(0); // default value + nameAttr = pullParser.getAttributeValue(1); // name value + typeAttr = pullParser.getAttributeValue(2); // type value + // set attributes to ParameterMetadataPX4 + metadataPX4.setDefaultAttr(defaultAttr); + metadataPX4.setName(nameAttr); + metadataPX4.setTypeAttr(typeAttr); + + } else { + if (metadataPX4 != null) { + addMetaDataPropertyPX4(metadataPX4, name, pullParser.nextText()); + } + } + } + break; + + case XmlPullParser.END_TAG: + if (TextUtils.equals(metadataType, name)) { + return; + } else if (metadataPX4 != null && metadataPX4.getName().equals(nameAttr)) { + metadataMap.put(metadataPX4.getName(), metadataPX4); + metadataPX4 = null; + } + break; + default: + break; + } + + type = pullParser.next(); + } + } + + + private static void addMetaDataPropertyPX4(ParameterMetadataPX4 metaData, String labelName, String text) { + switch (labelName) { + case METADATA_DISPLAYNAME_PX4: + metaData.setDisplayName(text); + break; + case METADATA_DESCRIPTION_PX4: + metaData.setDescription(text); + break; + case METADATA_UNITS_PX4: + metaData.setUnits(text); + break; + case METADATA_DECIMAL_PX4: + metaData.setDecimal(text); + break; + case METADATA_RANGE_MIN_PX4: + handleMinRange(metaData, text); + break; + case METADATA_RANGE_MAX_PX4: + handleMaxRange(metaData, text); + break; + + } + } + + private static void handleMinRange(ParameterMetadataPX4 metadataPX4, String text) { + // if the type is int32, it indicates that it can be converted to 'values' + if (TextUtils.equals(metadataPX4.getTypeAttr(), METADATA_TYPE_PX4_INT32)) { + if (minListInt32.isEmpty()) { + minListInt32.add(0, Integer.parseInt(text)); + } + } + // if the type is float, it indicates that it only can be converted to 'range' + else if (TextUtils.equals(metadataPX4.getTypeAttr(), METADATA_TYPE_PX4_FLOAT)) { + if (minListFloat.isEmpty()) { + minListFloat.add(0, Float.parseFloat(text)); + } + } + metadataPX4.setMin(text); + } + + private static void handleMaxRange(ParameterMetadataPX4 metadataPX4, String text) { + // set max value + metadataPX4.setMax(text); + // if the type is int32, it indicates that it can be converted to 'values' + if (TextUtils.equals(metadataPX4.getTypeAttr(), METADATA_TYPE_PX4_INT32)) { + if (!minListInt32.isEmpty()) { + int min = minListInt32.get(0); + int max = Integer.parseInt(text); + // if the diff is between 2, that can be chose, else fill in manually + if (min == 0 || min == -1) { + if (max - min <= 2) { + String value = ""; + for (int i = min; i <= max; i++) { + if (i != max) { + value = value + i + ": ,"; + } else { + value = value + i + ": "; + } + } + metadataPX4.setValues(value); + } + } else { + String range = min + " " + max; + metadataPX4.setRange(range); + } + minListInt32.clear(); + } + } + // if the type is float, it indicates that it only can be converted to 'range' + else if (TextUtils.equals(metadataPX4.getTypeAttr(), METADATA_TYPE_PX4_FLOAT)) { + if (!minListFloat.isEmpty()) { + float min = minListFloat.get(0); + String range = min + " " + text; + metadataPX4.setRange(range); + } + minListFloat.clear(); + } + } +} \ No newline at end of file From 91888bf8bc09a554ef03d9f0a3559306fd10ae7e Mon Sep 17 00:00:00 2001 From: fredia Date: Fri, 19 Feb 2016 09:57:08 -0800 Subject: [PATCH 13/37] Add the option to start the camera trigger after the first survey waypoint --- ClientLib/build.gradle | 4 +-- .../drone/mission/item/complex/Survey.java | 21 +++++++++++ ClientLib/src/main/res/values/version.xml | 2 +- .../core/mission/survey/SurveyImpl.java | 35 ++++++++++++++++--- .../services/android/utils/ProxyUtils.java | 4 +++ 5 files changed, 59 insertions(+), 7 deletions(-) diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index 027e9c6181..f9c354697a 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -3,8 +3,8 @@ apply plugin: 'com.android.library' ext { VERSION_MAJOR = 2 VERSION_MINOR = 8 - VERSION_PATCH = 0 - VERSION_SUFFIX = "release" + VERSION_PATCH = 1 + VERSION_SUFFIX = "alpha1" 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/services/android/lib/drone/mission/item/complex/Survey.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/Survey.java index faf6cf7a49..f55a044ba4 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/Survey.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/Survey.java @@ -27,6 +27,7 @@ public class Survey extends MissionItem implements MissionItem.ComplexItem gridPoints = new ArrayList(); private List cameraLocations = new ArrayList(); private boolean isValid; + private boolean startCameraBeforeFirstWaypoint; public Survey(){ this(MissionItemType.SURVEY); @@ -48,6 +49,7 @@ public void copy(Survey source){ this.gridPoints = copyPointsList(source.gridPoints); this.cameraLocations = copyPointsList(source.cameraLocations); this.isValid = source.isValid; + this.startCameraBeforeFirstWaypoint = source.startCameraBeforeFirstWaypoint; } private List copyPointsList(List copy){ @@ -115,6 +117,23 @@ public void setCameraLocations(List cameraLocations) { this.cameraLocations = cameraLocations; } + /** + * @since 2.8.1 + * @return true if the camera trigger should be started before reaching the first survey waypoint. + */ + public boolean isStartCameraBeforeFirstWaypoint() { + return startCameraBeforeFirstWaypoint; + } + + /** + * Enable to start the camera trigger before reaching the first survey waypoint. + * @since 2.8.1 + * @param startCameraBeforeFirstWaypoint + */ + public void setStartCameraBeforeFirstWaypoint(boolean startCameraBeforeFirstWaypoint) { + this.startCameraBeforeFirstWaypoint = startCameraBeforeFirstWaypoint; + } + public int getCameraCount() { return getCameraLocations().size(); } @@ -128,6 +147,7 @@ public void writeToParcel(Parcel dest, int flags) { dest.writeTypedList(gridPoints); dest.writeTypedList(cameraLocations); dest.writeByte(isValid ? (byte) 1 : (byte) 0); + dest.writeByte(startCameraBeforeFirstWaypoint ? (byte) 1: (byte) 0); } protected Survey(Parcel in) { @@ -138,6 +158,7 @@ protected Survey(Parcel in) { in.readTypedList(gridPoints, LatLong.CREATOR); in.readTypedList(cameraLocations, LatLong.CREATOR); this.isValid = in.readByte() != 0; + this.startCameraBeforeFirstWaypoint = in.readByte() != 0; } @Override diff --git a/ClientLib/src/main/res/values/version.xml b/ClientLib/src/main/res/values/version.xml index f377e23849..b0f6ed4844 100644 --- a/ClientLib/src/main/res/values/version.xml +++ b/ClientLib/src/main/res/values/version.xml @@ -1,4 +1,4 @@ - 20808 + 20809 \ No newline at end of file diff --git a/ServiceApp/src/org/droidplanner/services/android/core/mission/survey/SurveyImpl.java b/ServiceApp/src/org/droidplanner/services/android/core/mission/survey/SurveyImpl.java index 2b92460510..b118f7d6d9 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/mission/survey/SurveyImpl.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/mission/survey/SurveyImpl.java @@ -24,6 +24,8 @@ public class SurveyImpl extends MissionItemImpl { public SurveyData surveyData = new SurveyData(); public Grid grid; + private boolean startCameraBeforeFirstWaypoint; + public SurveyImpl(Mission mission, List points) { super(mission); polygon.addPoints(points); @@ -33,6 +35,14 @@ public void update(double angle, double altitude, double overlap, double sidelap surveyData.update(angle, altitude, overlap, sidelap); } + public boolean isStartCameraBeforeFirstWaypoint() { + return startCameraBeforeFirstWaypoint; + } + + public void setStartCameraBeforeFirstWaypoint(boolean startCameraBeforeFirstWaypoint) { + this.startCameraBeforeFirstWaypoint = startCameraBeforeFirstWaypoint; + } + public void setCameraInfo(CameraInfo camera) { surveyData.setCameraInfo(camera); } @@ -51,9 +61,7 @@ public List packMissionItem() { List list = new ArrayList(); build(); - list.addAll((new CameraTriggerImpl(mission, surveyData.getLongitudinalPictureDistance())).packMissionItem()); - packGridPoints(list); - list.addAll((new CameraTriggerImpl(mission, (0.0)).packMissionItem())); + packSurveyPoints(list); return list; } catch (Exception e) { @@ -61,12 +69,31 @@ public List packMissionItem() { } } - private void packGridPoints(List list) { + private void packSurveyPoints(List list) { + //Generate the camera trigger + CameraTriggerImpl camTrigger = new CameraTriggerImpl(mission, surveyData.getLongitudinalPictureDistance()); + + //Add it if the user wants it to start before the first waypoint. + if(startCameraBeforeFirstWaypoint){ + list.addAll(camTrigger.packMissionItem()); + } + final double altitude = surveyData.getAltitude(); + + //Add the camera trigger after the first waypoint if it wasn't added before. + boolean addToFirst = !startCameraBeforeFirstWaypoint; + for (LatLong point : grid.gridPoints) { msg_mission_item mavMsg = getSurveyPoint(point, altitude); list.add(mavMsg); + + if(addToFirst){ + list.addAll(camTrigger.packMissionItem()); + addToFirst = false; + } } + + list.addAll((new CameraTriggerImpl(mission, (0.0)).packMissionItem())); } protected msg_mission_item getSurveyPoint(LatLong point, double altitude){ diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java b/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java index c999890081..d69eb63456 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/ProxyUtils.java @@ -232,6 +232,7 @@ public static MissionItemImpl getMissionItemImpl(Mission mission, MissionItem pr SurveyDetail surveyDetail = proxy.getSurveyDetail(); SurveyImpl temp = new SurveyImpl(mission, proxy.getPolygonPoints()); + temp.setStartCameraBeforeFirstWaypoint(proxy.isStartCameraBeforeFirstWaypoint()); if (surveyDetail != null) { CameraDetail cameraDetail = surveyDetail.getCameraDetail(); @@ -256,6 +257,7 @@ public static MissionItemImpl getMissionItemImpl(Mission mission, MissionItem pr SurveyDetail surveyDetail = proxy.getSurveyDetail(); SplineSurveyImpl temp = new SplineSurveyImpl(mission, proxy.getPolygonPoints()); + temp.setStartCameraBeforeFirstWaypoint(proxy.isStartCameraBeforeFirstWaypoint()); if (surveyDetail != null) { CameraDetail cameraDetail = surveyDetail.getCameraDetail(); @@ -413,6 +415,7 @@ public static MissionItem getProxyMissionItem(MissionItemImpl itemImpl) { } Survey temp = new Survey(); + temp.setStartCameraBeforeFirstWaypoint(source.isStartCameraBeforeFirstWaypoint()); temp.setValid(isValid); temp.setSurveyDetail(getSurveyDetail(source.surveyData)); temp.setPolygonPoints((source.polygon.getPoints())); @@ -439,6 +442,7 @@ public static MissionItem getProxyMissionItem(MissionItemImpl itemImpl) { } Survey temp = new Survey(); + temp.setStartCameraBeforeFirstWaypoint(source.isStartCameraBeforeFirstWaypoint()); temp.setValid(isValid); temp.setSurveyDetail(getSurveyDetail(source.surveyData)); temp.setPolygonPoints((source.polygon.getPoints())); From be6d54457baa460f9bf97d298dcd0c63b0489f02 Mon Sep 17 00:00:00 2001 From: fredia Date: Fri, 19 Feb 2016 13:20:05 -0800 Subject: [PATCH 14/37] Bump the version --- ServiceApp/build.gradle | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ServiceApp/build.gradle b/ServiceApp/build.gradle index babb913052..d06fe567a3 100644 --- a/ServiceApp/build.gradle +++ b/ServiceApp/build.gradle @@ -39,8 +39,8 @@ dependencies { //Decomposed version name and code (https://plus.google.com/+JakeWharton/posts/6f5TcVPRZij) def versionMajor = 1 def versionMinor = 5 -def versionPatch = 1 -def versionBuild = 12 //bump for dogfood builds, public betas, etc. +def versionPatch = 2 +def versionBuild = 0 //bump for dogfood builds, public betas, etc. def versionPrefix = "3DR Services v" //Log levels values From 99220524723e842a57fcecb812e042d599eeb401 Mon Sep 17 00:00:00 2001 From: fredia Date: Sun, 21 Feb 2016 18:34:41 -0800 Subject: [PATCH 15/37] Fix bug preventing building the survey mission item when disconnect from the vehicle --- .../src/org/droidplanner/services/android/api/DroneApi.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index a86d0bd3bf..0835681214 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -311,7 +311,7 @@ public void executeAction(Action action, ICommandListener listener) throws Remot // MISSION ACTIONS case MissionActions.ACTION_BUILD_COMPLEX_MISSION_ITEM: - if (drone instanceof MavLinkDrone) { + if (drone instanceof MavLinkDrone || drone == null) { CommonApiUtils.buildComplexMissionItem((MavLinkDrone) drone, data); } else { CommonApiUtils.postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); From bd1a745dea3bbb2d5e44a6b6ebee2c251e172442 Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Mon, 22 Feb 2016 15:10:26 -0800 Subject: [PATCH 16/37] Updated per comments --- .../services/android/api/DroneApi.java | 6 +++++- .../utils/connection/WifiConnectionHandler.java | 16 ++++++++-------- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index b2a5ceadd6..6d41c84065 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -202,7 +202,11 @@ public void connect(ConnectionParameter connParams) { connParams = checkConnectionParameter(connParams); if (!connParams.equals(this.connectionParams)) { if (this.droneMgr != null) { - throw new ConnectionException("Connection already started with different connection parameters"); + LinkConnectionStatus connectionStatus = LinkConnectionStatus + .newFailedConnectionStatus(LinkConnectionStatus.ADDRESS_IN_USE, + "Connection already started with different connection parameters"); + onConnectionStatus(connectionStatus); + return; } this.connectionParams = connParams; diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java index d29beb1b0d..59debd0d37 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java @@ -76,7 +76,7 @@ public void onReceive(Context context, Intent intent) { Timber.d("Supplicant state changed error %s with state %s and ssid %s", supplicationError, supState, ssid); if (supplicationError == WifiManager.ERROR_AUTHENTICATING) { if (NetworkUtils.isSoloNetwork(ssid)) { - notifyWifiConnectionFailed(ssid); + notifyWifiConnectionFailed(); WifiConfiguration wifiConfig = getWifiConfigs(ssid); if (wifiConfig != null) { wifiMgr.removeNetwork(wifiConfig.networkId); @@ -137,7 +137,7 @@ public void onReceive(Context context, Intent intent) { private final Object netReq; private final Object netReqCb; - private final AtomicReference connectedSoloWifi = new AtomicReference<>(""); + private final AtomicReference connectedWifi = new AtomicReference<>(""); private final Context context; @@ -250,7 +250,7 @@ public void stop() { private void resetNetworkBindings(ConnectivityManager.NetworkCallback netCb) { Timber.i("Unregistering network callbacks."); - connectedSoloWifi.set(NetworkUtils.getCurrentWifiLink(context)); + connectedWifi.set(NetworkUtils.getCurrentWifiLink(context)); try { connMgr.unregisterNetworkCallback(netCb); } catch (IllegalArgumentException e) { @@ -437,7 +437,7 @@ public static boolean isSoloWifi(String wifiSsid) { private void setDefaultNetworkIfNecessary(String wifiSsid) { final String trimmedSsid = trimWifiSsid(wifiSsid); - if (!trimmedSsid.equals(connectedSoloWifi.get())) { + if (!trimmedSsid.equals(connectedWifi.get())) { if (isConnected(wifiSsid)) { notifyWifiConnected(wifiSsid); return; @@ -455,7 +455,7 @@ private void setDefaultNetworkIfNecessary(String wifiSsid) { } else { notifyWifiConnected(trimmedSsid); } - connectedSoloWifi.set(trimmedSsid); + connectedWifi.set(trimmedSsid); } } @@ -473,9 +473,9 @@ private void notifyWifiConnecting() { private void notifyWifiDisconnected() { if (listener != null) { - listener.onWifiDisconnected(connectedSoloWifi.get()); + listener.onWifiDisconnected(connectedWifi.get()); } - connectedSoloWifi.set(""); + connectedWifi.set(""); } private void notifyWifiScanResultsAvailable(List results) { @@ -484,7 +484,7 @@ private void notifyWifiScanResultsAvailable(List results) { } } - private void notifyWifiConnectionFailed(String ssid) { + private void notifyWifiConnectionFailed() { if (listener != null) { LinkConnectionStatus linkConnectionStatus = LinkConnectionStatus .newFailedConnectionStatus(LinkConnectionStatus.INVALID_CREDENTIALS, null); From 8067d5004fb904daf7ae6c130a59fd2f2944a5be Mon Sep 17 00:00:00 2001 From: fredia Date: Tue, 23 Feb 2016 15:15:22 -0800 Subject: [PATCH 17/37] Bump version for the dronekit-android client library --- ClientLib/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index f9c354697a..f91d31a55d 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -4,7 +4,7 @@ ext { VERSION_MAJOR = 2 VERSION_MINOR = 8 VERSION_PATCH = 1 - VERSION_SUFFIX = "alpha1" + VERSION_SUFFIX = "beta1" PUBLISH_ARTIFACT_ID = 'dronekit-android' PUBLISH_VERSION = generateVersionName("", VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH, VERSION_SUFFIX) From 0a79bf26891809f5d1c6d3318f85998224d40b68 Mon Sep 17 00:00:00 2001 From: fredia Date: Tue, 23 Feb 2016 15:20:08 -0800 Subject: [PATCH 18/37] Update the supported build variants --- ServiceApp/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ServiceApp/build.gradle b/ServiceApp/build.gradle index d06fe567a3..0322e4ea6c 100644 --- a/ServiceApp/build.gradle +++ b/ServiceApp/build.gradle @@ -135,6 +135,7 @@ android { case "release": switch (flavorName) { case "alpha": + case "beta": case "prod": ignoreVariant = false break @@ -153,7 +154,6 @@ android { case "staging": switch (flavorName) { case "beta": - case "alpha": ignoreVariant = false break } From 24a5a5aa479c31bdba5ace1c8b987c654421df43 Mon Sep 17 00:00:00 2001 From: fredia Date: Tue, 23 Feb 2016 19:18:30 -0800 Subject: [PATCH 19/37] Update based on review --- .../android/client/apis/ExperimentalApi.java | 145 +++++++++++------- .../lib/drone/action/ExperimentalActions.java | 2 + .../services/android/api/DroneApi.java | 7 +- .../android/utils/video/VideoManager.java | 10 +- 4 files changed, 99 insertions(+), 65 deletions(-) diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java index 068eb9474a..3d00ae9826 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java @@ -2,12 +2,14 @@ import android.os.Bundle; import android.os.Handler; +import android.text.TextUtils; import android.util.Log; import com.o3dr.android.client.Drone; import com.o3dr.android.client.utils.connection.IpConnectionListener; import com.o3dr.android.client.utils.connection.UdpConnection; import com.o3dr.services.android.lib.coordinate.LatLongAlt; +import com.o3dr.services.android.lib.drone.action.ExperimentalActions; import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.mavlink.MavlinkMessageWrapper; import com.o3dr.services.android.lib.model.AbstractCommandListener; @@ -43,12 +45,6 @@ public ExperimentalApi build(Drone drone) { private final CapabilityApi capabilityChecker; - public static final String ACTION_START_VIDEO_STREAM_FOR_OBSERVER = - "com.o3dr.services.android.lib.drone.companion.solo.action.camera.START_VIDEO_STREAM_FOR_OBSERVER"; - - public static final String ACTION_STOP_VIDEO_STREAM_FOR_OBSERVER = - "com.o3dr.services.android.lib.drone.companion.solo.action.camera.STOP_VIDEO_STREAM_FOR_OBSERVER"; - private final VideoStreamObserver videoStreamObserver; /** @@ -175,7 +171,7 @@ public void setServo(int channel, int pwm, AbstractCommandListener listener) { * @param tag Video tag. * @param callback Video stream observer callback. * - * @since 2.5.0 + * @since 2.8.1 */ public void startVideoStream(final String tag, final IVideoStreamCallback callback) { if (callback == null) { @@ -183,46 +179,46 @@ public void startVideoStream(final String tag, final IVideoStreamCallback callba } capabilityChecker.checkFeatureSupport(CapabilityApi.FeatureIds.SOLO_VIDEO_STREAMING, - new CapabilityApi.FeatureSupportListener() { - @Override - public void onFeatureSupportResult(String featureId, int result, Bundle resultInfo) { - final AbstractCommandListener listener = new AbstractCommandListener() { - @Override - public void onSuccess() { - // Start VideoStreamObserver to connect to vehicle video stream and receive - // video stream packets. - videoStreamObserver.setCallback(callback); - videoStreamObserver.start(); - - videoStreamObserver.getCallback().onVideoStreamConnecting(); - } - - @Override - public void onError(int executionError) { - videoStreamObserver.getCallback().onError(executionError); + new CapabilityApi.FeatureSupportListener() { + @Override + public void onFeatureSupportResult(String featureId, int result, Bundle resultInfo) { + final AbstractCommandListener listener = new AbstractCommandListener() { + @Override + public void onSuccess() { + // Start VideoStreamObserver to connect to vehicle video stream and receive + // video stream packets. + videoStreamObserver.setCallback(callback); + videoStreamObserver.start(); + + videoStreamObserver.getCallback().onVideoStreamConnecting(); + } + + @Override + public void onError(int executionError) { + videoStreamObserver.getCallback().onError(executionError); + } + + @Override + public void onTimeout() { + videoStreamObserver.getCallback().onTimeout(); + } + }; + + switch (result) { + case CapabilityApi.FEATURE_SUPPORTED: + startVideoStreamForObserver(tag, listener); + break; + + case CapabilityApi.FEATURE_UNSUPPORTED: + postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); + break; + + default: + postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); + break; } - - @Override - public void onTimeout() { - videoStreamObserver.getCallback().onTimeout(); - } - }; - - switch (result) { - case CapabilityApi.FEATURE_SUPPORTED: - startVideoStreamForObserver(tag, listener); - break; - - case CapabilityApi.FEATURE_UNSUPPORTED: - postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); - break; - - default: - postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); - break; } - } - }); + }); } /** @@ -230,7 +226,7 @@ public void onTimeout() { * * @param tag Video tag. * - * @since 2.5.0 + * @since 2.8.1 */ public void stopVideoStream(final String tag) { capabilityChecker.checkFeatureSupport(CapabilityApi.FeatureIds.SOLO_VIDEO_STREAMING, @@ -273,19 +269,28 @@ public void onTimeout() { }); } + /** + * Prepending 'observer' to the tag for differentiation + * @param tag + * @return + */ + private String getObserverTag(String tag){ + return "observer" + (TextUtils.isEmpty(tag) ? "" : "." + tag); + } + /** * Attempt to grab ownership and start the video stream from the connected drone. Can fail if * the video stream is already owned by another client. * * @param tag Video tag. * @param listener Register a listener to receive update of the command execution status. - * @since 2.6.8 + * @since 2.8.1 */ private void startVideoStreamForObserver(final String tag, AbstractCommandListener listener) { final Bundle params = new Bundle(); - params.putString(EXTRA_VIDEO_TAG, tag); + params.putString(EXTRA_VIDEO_TAG, getObserverTag(tag)); - drone.performAsyncActionOnDroneThread(new Action(ACTION_START_VIDEO_STREAM_FOR_OBSERVER, + drone.performAsyncActionOnDroneThread(new Action(ExperimentalActions.ACTION_START_VIDEO_STREAM_FOR_OBSERVER, params), listener); } @@ -294,13 +299,13 @@ private void startVideoStreamForObserver(final String tag, AbstractCommandListen * * @param tag Video tag. * @param listener Register a listener to receive update of the command execution status. - * @since 2.6.8 + * @since 2.8.1 */ private void stopVideoStreamForObserver(final String tag, AbstractCommandListener listener) { final Bundle params = new Bundle(); - params.putString(EXTRA_VIDEO_TAG, tag); + params.putString(EXTRA_VIDEO_TAG, getObserverTag(tag)); - drone.performAsyncActionOnDroneThread(new Action(ACTION_STOP_VIDEO_STREAM_FOR_OBSERVER, params), + drone.performAsyncActionOnDroneThread(new Action(ExperimentalActions.ACTION_STOP_VIDEO_STREAM_FOR_OBSERVER, params), listener); } @@ -315,7 +320,26 @@ private static class VideoStreamObserver implements IpConnectionListener { private static final int SOLO_STREAM_UDP_PORT = 5600; private UdpConnection linkConn; - private Handler handler; + private final Handler handler; + + private final Runnable onVideoStreamConnected = new Runnable() { + @Override + public void run() { + handler.removeCallbacks(this); + if(callback != null) + callback.onVideoStreamConnected(); + } + }; + + private final Runnable onVideoStreamDisconnected = new Runnable() { + @Override + public void run() { + handler.removeCallbacks(this); + if(callback != null){ + callback.onVideoStreamDisconnected(); + } + } + }; private IVideoStreamCallback callback; @@ -369,8 +393,7 @@ public void stop() { public void onIpConnected() { Log.d(TAG, "Connected to video stream"); - callback.onVideoStreamConnected(); - + handler.post(onVideoStreamConnected); handler.removeCallbacks(reconnectTask); } @@ -378,14 +401,18 @@ public void onIpConnected() { public void onIpDisconnected() { Log.d(TAG, "Video stream disconnected"); - callback.onVideoStreamDisconnected(); - + handler.post(onVideoStreamDisconnected); handler.postDelayed(reconnectTask, RECONNECT_COUNTDOWN_IN_MILLIS); } @Override - public void onPacketReceived(ByteBuffer packetBuffer) { - callback.onVideoStreamPacketReceived(packetBuffer.array(), packetBuffer.limit()); + public void onPacketReceived(final ByteBuffer packetBuffer) { + handler.post(new Runnable() { + @Override + public void run() { + callback.onVideoStreamPacketReceived(packetBuffer.array(), packetBuffer.limit()); + } + }); } } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/ExperimentalActions.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/ExperimentalActions.java index 025c80174e..f537e11462 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/ExperimentalActions.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/action/ExperimentalActions.java @@ -29,4 +29,6 @@ private ExperimentalActions(){} public static final String EXTRA_SERVO_CHANNEL = "extra_servo_channel"; public static final String EXTRA_SERVO_PWM = "extra_servo_PWM"; + public static final String ACTION_START_VIDEO_STREAM_FOR_OBSERVER = Utils.PACKAGE_NAME + ".action.camera.START_VIDEO_STREAM_FOR_OBSERVER"; + public static final String ACTION_STOP_VIDEO_STREAM_FOR_OBSERVER = Utils.PACKAGE_NAME + ".action.camera.STOP_VIDEO_STREAM_FOR_OBSERVER"; } diff --git a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java index 015e157a8f..51c95609f2 100644 --- a/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java +++ b/ServiceApp/src/org/droidplanner/services/android/api/DroneApi.java @@ -12,11 +12,10 @@ import com.MAVLink.Messages.MAVLinkMessage; import com.MAVLink.ardupilotmega.msg_mag_cal_progress; import com.MAVLink.ardupilotmega.msg_mag_cal_report; -import com.o3dr.android.client.apis.ExperimentalApi; import com.o3dr.services.android.lib.drone.action.CameraActions; import com.o3dr.services.android.lib.coordinate.LatLongAlt; -import com.o3dr.services.android.lib.drone.action.CameraActions; import com.o3dr.services.android.lib.drone.action.ConnectionActions; +import com.o3dr.services.android.lib.drone.action.ExperimentalActions; import com.o3dr.services.android.lib.drone.attribute.AttributeEvent; import com.o3dr.services.android.lib.drone.attribute.AttributeEventExtra; import com.o3dr.services.android.lib.drone.attribute.AttributeType; @@ -305,7 +304,7 @@ public void executeAction(Action action, ICommandListener listener) throws Remot break; } - case ExperimentalApi.ACTION_START_VIDEO_STREAM_FOR_OBSERVER: { + case ExperimentalActions.ACTION_START_VIDEO_STREAM_FOR_OBSERVER: { String videoTag = data.getString(CameraActions.EXTRA_VIDEO_TAG, ""); CommonApiUtils.startVideoStreamForObserver(drone, ownerId, videoTag, listener); break; @@ -317,7 +316,7 @@ public void executeAction(Action action, ICommandListener listener) throws Remot break; } - case ExperimentalApi.ACTION_STOP_VIDEO_STREAM_FOR_OBSERVER: { + case ExperimentalActions.ACTION_STOP_VIDEO_STREAM_FOR_OBSERVER: { String videoTag = data.getString(CameraActions.EXTRA_VIDEO_TAG, ""); CommonApiUtils.stopVideoStreamForObserver(drone, ownerId, videoTag, listener); break; diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java index f57f32a615..7c4fff8a12 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java @@ -41,7 +41,7 @@ public class VideoManager implements IpConnectionListener { public static final int ARTOO_UDP_PORT = 5600; private static final int UDP_BUFFER_SIZE = 1500; - private static final AtomicBoolean videoStreamObserverUsed = new AtomicBoolean(false); + private final AtomicBoolean videoStreamObserverUsed = new AtomicBoolean(false); public interface LinkListener { void onLinkConnected(); @@ -488,7 +488,13 @@ public void tryStoppingVideoStream(String parentId) { if (videoOwner.equals(parentId)){ Timber.d("Stopping video owned by %s", parentId); - stopVideoStream(parentId, videoTagRef.get(), null); + + if(videoStreamObserverUsed.get()){ + stopVideoStreamForObserver(parentId, videoTagRef.get(), null); + } + else { + stopVideoStream(parentId, videoTagRef.get(), null); + } } } From 3ee01f2a2c4fb7c39b3faa92560e0070e7cc5666 Mon Sep 17 00:00:00 2001 From: fredia Date: Tue, 23 Feb 2016 19:26:04 -0800 Subject: [PATCH 20/37] Bump for beta 1 release --- ServiceApp/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ServiceApp/build.gradle b/ServiceApp/build.gradle index 0322e4ea6c..d394b0c25b 100644 --- a/ServiceApp/build.gradle +++ b/ServiceApp/build.gradle @@ -40,7 +40,7 @@ dependencies { def versionMajor = 1 def versionMinor = 5 def versionPatch = 2 -def versionBuild = 0 //bump for dogfood builds, public betas, etc. +def versionBuild = 1 //bump for dogfood builds, public betas, etc. def versionPrefix = "3DR Services v" //Log levels values From 60b2fcf6a74eb2e980a85d2660d808cae5321807 Mon Sep 17 00:00:00 2001 From: fredia Date: Wed, 24 Feb 2016 09:35:34 -0800 Subject: [PATCH 21/37] Rename `onVideoStreamPacketReceived()` callback to better reflect its use --- .../android/client/apis/ExperimentalApi.java | 82 ++++++++++--------- .../o3dr/sample/hellodrone/MainActivity.java | 2 +- 2 files changed, 44 insertions(+), 40 deletions(-) diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java index 3d00ae9826..7aee0840ba 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java @@ -230,43 +230,43 @@ public void onTimeout() { */ public void stopVideoStream(final String tag) { capabilityChecker.checkFeatureSupport(CapabilityApi.FeatureIds.SOLO_VIDEO_STREAMING, - new CapabilityApi.FeatureSupportListener() { - @Override - public void onFeatureSupportResult(String featureId, int result, Bundle resultInfo) { - final AbstractCommandListener listener = new AbstractCommandListener() { - @Override - public void onSuccess() { - videoStreamObserver.getCallback().onVideoStreamDisconnecting(); - - videoStreamObserver.stop(); - } + new CapabilityApi.FeatureSupportListener() { + @Override + public void onFeatureSupportResult(String featureId, int result, Bundle resultInfo) { + final AbstractCommandListener listener = new AbstractCommandListener() { + @Override + public void onSuccess() { + videoStreamObserver.getCallback().onVideoStreamDisconnecting(); - @Override - public void onError(int executionError) { - videoStreamObserver.getCallback().onError(executionError); - } + videoStreamObserver.stop(); + } - @Override - public void onTimeout() { - videoStreamObserver.getCallback().onTimeout(); - } - }; + @Override + public void onError(int executionError) { + videoStreamObserver.getCallback().onError(executionError); + } - switch (result) { - case CapabilityApi.FEATURE_SUPPORTED: - stopVideoStreamForObserver(tag, listener); - break; + @Override + public void onTimeout() { + videoStreamObserver.getCallback().onTimeout(); + } + }; + + switch (result) { + case CapabilityApi.FEATURE_SUPPORTED: + stopVideoStreamForObserver(tag, listener); + break; - case CapabilityApi.FEATURE_UNSUPPORTED: - postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); - break; + case CapabilityApi.FEATURE_UNSUPPORTED: + postErrorEvent(CommandExecutionError.COMMAND_UNSUPPORTED, listener); + break; - default: - postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); - break; + default: + postErrorEvent(CommandExecutionError.COMMAND_FAILED, listener); + break; + } } - } - }); + }); } /** @@ -306,7 +306,7 @@ private void stopVideoStreamForObserver(final String tag, AbstractCommandListene params.putString(EXTRA_VIDEO_TAG, getObserverTag(tag)); drone.performAsyncActionOnDroneThread(new Action(ExperimentalActions.ACTION_STOP_VIDEO_STREAM_FOR_OBSERVER, params), - listener); + listener); } /** @@ -407,12 +407,7 @@ public void onIpDisconnected() { @Override public void onPacketReceived(final ByteBuffer packetBuffer) { - handler.post(new Runnable() { - @Override - public void run() { - callback.onVideoStreamPacketReceived(packetBuffer.array(), packetBuffer.limit()); - } - }); + callback.onAsyncVideoStreamPacketReceived(packetBuffer.array(), packetBuffer.limit()); } } @@ -420,6 +415,9 @@ public void run() { * Callback for directly observing video stream. */ public interface IVideoStreamCallback { + /** + * Invoked upon + */ void onVideoStreamConnecting(); void onVideoStreamConnected(); @@ -432,6 +430,12 @@ public interface IVideoStreamCallback { void onTimeout(); - void onVideoStreamPacketReceived(byte[] data, int dataSize); + /** + * Invoked upon receipt of the video stream data packet. + * This callback will be invoked on a background thread to avoid blocking the main thread while processing the received data + * @param data Video stream data packet + * @param dataSize Size of the video stream data + */ + void onAsyncVideoStreamPacketReceived(byte[] data, int dataSize); } } diff --git a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java index 6f30a3022a..830643a00d 100644 --- a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java +++ b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java @@ -688,7 +688,7 @@ public void onTimeout() { } @Override - public void onVideoStreamPacketReceived(byte[] data, int dataSize) { + public void onAsyncVideoStreamPacketReceived(byte[] data, int dataSize) { mediaCodecManager.onInputDataReceived(data, dataSize); } }); From 1e0d075097bf35c77196cfaf03c2f78be41c05f8 Mon Sep 17 00:00:00 2001 From: fredia Date: Wed, 24 Feb 2016 09:54:57 -0800 Subject: [PATCH 22/37] Add javadocs for the `IVideoStreamCallback` --- .../android/client/apis/ExperimentalApi.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java b/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java index 7aee0840ba..8dbbe15f3b 100644 --- a/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/apis/ExperimentalApi.java @@ -416,18 +416,34 @@ public void onPacketReceived(final ByteBuffer packetBuffer) { */ public interface IVideoStreamCallback { /** - * Invoked upon + * Invoked when opening the connection to the video stream endpoint */ void onVideoStreamConnecting(); + /** + * Invoked when connected to the video stream endpoint + */ void onVideoStreamConnected(); + /** + * Invoked when closing the connection to the video stream endpoint + */ void onVideoStreamDisconnecting(); + /** + * Invoked when disconnected from the video stream endpoint + */ void onVideoStreamDisconnected(); + /** + * Invoked when detecting an error while connecting to the video stream endpoint + * @param executionError + */ void onError(int executionError); + /** + * Invoked when the connection to the video stream endpoint times out + */ void onTimeout(); /** From 8b3aadfbfb19dfcfba9ecf0e6cf8b51b56f7e99f Mon Sep 17 00:00:00 2001 From: fredia Date: Wed, 24 Feb 2016 10:10:57 -0800 Subject: [PATCH 23/37] Rename NALU* classes to Nalu* Add `NaluChunkListener` listener to `MediaCodecManager` to generify access to the processed nalu chunk --- .../utils/video/MediaCodecManager.java | 29 +++++++++++++------ .../android/utils/video/NALUChunk.java | 4 +-- .../utils/video/NALUChunkAssembler.java | 25 ++++++++-------- .../android/utils/video/StreamRecorder.java | 8 ++--- .../android/utils/video/VideoManager.java | 3 +- 5 files changed, 40 insertions(+), 29 deletions(-) diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/MediaCodecManager.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/MediaCodecManager.java index 19664572da..765aa0e53d 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/MediaCodecManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/video/MediaCodecManager.java @@ -19,6 +19,10 @@ @TargetApi(Build.VERSION_CODES.JELLY_BEAN_MR2) public class MediaCodecManager { + public interface NaluChunkListener { + void onNaluChunkUpdated(NaluChunk parametersSet, NaluChunk dataChunk); + } + private static final String TAG = MediaCodecManager.class.getSimpleName(); private static final String MIME_TYPE = "video/avc"; @@ -91,20 +95,24 @@ public void run() { private final AtomicReference surfaceRef = new AtomicReference<>(); private final AtomicReference mediaCodecRef = new AtomicReference<>(); private final AtomicReference decoderListenerRef = new AtomicReference<>(); - private final NALUChunkAssembler naluChunkAssembler; + private final NaluChunkAssembler naluChunkAssembler; private final Handler handler; - private final StreamRecorder streamRecorder; + + private final AtomicReference naluChunkListenerRef = new AtomicReference<>(); private DequeueCodec dequeueRunner; - public MediaCodecManager(Handler handler, StreamRecorder recorder) { + public MediaCodecManager(Handler handler) { this.handler = handler; - this.naluChunkAssembler = new NALUChunkAssembler(); - this.streamRecorder = recorder; + this.naluChunkAssembler = new NaluChunkAssembler(); } - Surface getSurface(){ + public void setNaluChunkListener(NaluChunkListener naluChunkListener) { + this.naluChunkListenerRef.set(naluChunkListener); + } + + public Surface getSurface(){ return surfaceRef.get(); } @@ -158,7 +166,7 @@ public void onInputDataReceived(byte[] data, int dataSize) { if (isDecoding.get()) { if (processInputData.get()) { //Process the received buffer - NALUChunk naluChunk = naluChunkAssembler.assembleNALUChunk(data, dataSize); + NaluChunk naluChunk = naluChunkAssembler.assembleNALUChunk(data, dataSize); if (naluChunk != null) processNALUChunk(naluChunk); } else { @@ -170,7 +178,7 @@ public void onInputDataReceived(byte[] data, int dataSize) { } } - private boolean processNALUChunk(NALUChunk naluChunk) { + private boolean processNALUChunk(NaluChunk naluChunk) { if (naluChunk == null) return false; @@ -209,7 +217,10 @@ private boolean processNALUChunk(NALUChunk naluChunk) { totalLength += dataLength; } - streamRecorder.onNaluChunkUpdated(naluChunkAssembler.getParametersSet(), naluChunk); + NaluChunkListener naluChunkListener = naluChunkListenerRef.get(); + if(naluChunkListener != null){ + naluChunkListener.onNaluChunkUpdated(naluChunkAssembler.getParametersSet(), naluChunk); + } mediaCodec.queueInputBuffer(index, 0, totalLength, naluChunk.presentationTime, naluChunk.flags); } diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunk.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunk.java index 70e30d1b93..bb0a712038 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunk.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunk.java @@ -5,14 +5,14 @@ /** * Created by fhuya on 12/4/14. */ -public class NALUChunk { +public class NaluChunk { public static final byte[] START_CODE = {0, 0, 0, 1}; public static final int SPS_NAL_TYPE = 7; public static final int PPS_NAL_TYPE = 8; public final ByteBuffer[] payloads; - public NALUChunk(int payloadCount, int payloadSize, byte[] payloadInitData) { + public NaluChunk(int payloadCount, int payloadSize, byte[] payloadInitData) { this.payloads = new ByteBuffer[payloadCount]; for (int i = 0; i < payloadCount; i++) { this.payloads[i] = ByteBuffer.allocate(payloadSize); diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunkAssembler.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunkAssembler.java index 110e904dec..e9b842cbd3 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunkAssembler.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunkAssembler.java @@ -3,7 +3,6 @@ import android.annotation.TargetApi; import android.media.MediaCodec; import android.os.Build; -import android.util.Log; import java.nio.ByteBuffer; @@ -13,11 +12,11 @@ * Created by Fredia Huya-Kouadio on 6/1/15. */ @TargetApi(Build.VERSION_CODES.JELLY_BEAN_MR2) -public class NALUChunkAssembler { +class NaluChunkAssembler { - private final NALUChunk assembledNaluChunk; - private final NALUChunk paramsNaluChunk; - private final NALUChunk eosNaluChunk; + private final NaluChunk assembledNaluChunk; + private final NaluChunk paramsNaluChunk; + private final NaluChunk eosNaluChunk; /** * Stores the sps data so it can be concatenate with the pps data. @@ -31,14 +30,14 @@ public class NALUChunkAssembler { private final static int PPS_BUFFER_INDEX = 1; private boolean isPpsSet = false; - NALUChunkAssembler(){ - this.assembledNaluChunk = new NALUChunk(1, 1024 * 1024, NALUChunk.START_CODE); + NaluChunkAssembler(){ + this.assembledNaluChunk = new NaluChunk(1, 1024 * 1024, NaluChunk.START_CODE); - this.paramsNaluChunk = new NALUChunk(2, 256, NALUChunk.START_CODE); + this.paramsNaluChunk = new NaluChunk(2, 256, NaluChunk.START_CODE); this.paramsNaluChunk.type = 78; this.paramsNaluChunk.flags = MediaCodec.BUFFER_FLAG_CODEC_CONFIG; - this.eosNaluChunk = new NALUChunk(1, 0, null); + this.eosNaluChunk = new NaluChunk(1, 0, null); this.eosNaluChunk.flags = MediaCodec.BUFFER_FLAG_END_OF_STREAM; } @@ -56,7 +55,7 @@ private boolean areParametersSet() { return isSpsSet && isPpsSet; } - NALUChunk getEndOfStream(){ + NaluChunk getEndOfStream(){ return eosNaluChunk; } @@ -64,14 +63,14 @@ NALUChunk getEndOfStream(){ private int naluCounter = 0; private final static long DELTA_PRESENTATION_TIME = 42000L; - NALUChunk getParametersSet(){ + NaluChunk getParametersSet(){ if(areParametersSet()) return paramsNaluChunk; return null; } - NALUChunk assembleNALUChunk(byte[] buffer, int bufferLength) { + NaluChunk assembleNALUChunk(byte[] buffer, int bufferLength) { //The first 12 bytes are the rtp header. final byte nalHeaderByte = buffer[12]; @@ -113,7 +112,7 @@ NALUChunk assembleNALUChunk(byte[] buffer, int bufferLength) { case 8: //PPS parameters set. { ByteBuffer naluData; - if (nalType == NALUChunk.SPS_NAL_TYPE) { + if (nalType == NaluChunk.SPS_NAL_TYPE) { naluData = paramsNaluChunk.payloads[SPS_BUFFER_INDEX]; isSpsSet = true; } else { diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamRecorder.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamRecorder.java index ab7d494a60..936c7692f6 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamRecorder.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamRecorder.java @@ -5,7 +5,6 @@ import android.net.Uri; import android.os.Environment; import android.text.TextUtils; -import android.util.Log; import com.coremedia.iso.boxes.Container; import com.googlecode.mp4parser.FileDataSourceImpl; @@ -30,7 +29,7 @@ /** * Created by Fredia Huya-Kouadio on 11/22/15. */ -class StreamRecorder { +class StreamRecorder implements MediaCodecManager.NaluChunkListener{ private final AtomicReference recordingFilename = new AtomicReference<>(); private final AtomicBoolean areParametersSet = new AtomicBoolean(false); @@ -127,7 +126,8 @@ boolean disableRecording() { } //TODO: Maybe put this on a background thread to avoid blocking on the write to file. - void onNaluChunkUpdated(NALUChunk parametersSet, NALUChunk dataChunk) { + @Override + public void onNaluChunkUpdated(NaluChunk parametersSet, NaluChunk dataChunk) { if (isRecordingEnabled() && h264Writer != null) { if(areParametersSet.get()) { try { @@ -146,7 +146,7 @@ void onNaluChunkUpdated(NALUChunk parametersSet, NALUChunk dataChunk) { } } - private boolean writeNaluChunk(BufferedOutputStream bos, NALUChunk naluChunk) throws IOException { + private boolean writeNaluChunk(BufferedOutputStream bos, NaluChunk naluChunk) throws IOException { if(naluChunk == null) return false; diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java index 7c4fff8a12..193d5c2943 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java @@ -78,7 +78,8 @@ public VideoManager(Context context, Handler handler) { this.streamRecorder = new StreamRecorder(context); this.handler = handler; - this.mediaCodecManager = new MediaCodecManager(handler, streamRecorder); + this.mediaCodecManager = new MediaCodecManager(handler); + this.mediaCodecManager.setNaluChunkListener(streamRecorder); } private void enableLocalRecording(String filename) { From 7190a2019782ba89968357475838a728df8d2dd0 Mon Sep 17 00:00:00 2001 From: fredia Date: Wed, 24 Feb 2016 10:12:53 -0800 Subject: [PATCH 24/37] Remove unused `StreamingListener` interface --- .../android/utils/video/StreamingListener.java | 10 ---------- 1 file changed, 10 deletions(-) delete mode 100644 ServiceApp/src/org/droidplanner/services/android/utils/video/StreamingListener.java diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamingListener.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamingListener.java deleted file mode 100644 index 802d692494..0000000000 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamingListener.java +++ /dev/null @@ -1,10 +0,0 @@ -package org.droidplanner.services.android.utils.video; - -/** -* Created by fhuya on 12/4/14. -*/ -public interface StreamingListener { - void onStreamingStarted(); - - void onStreamingError(); -} From 5bad7361053c78275f80a59227bcb143d834b9d5 Mon Sep 17 00:00:00 2001 From: fredia Date: Wed, 24 Feb 2016 10:16:52 -0800 Subject: [PATCH 25/37] Move the media decoding logic to the client library --- .../client}/utils/video/DecoderListener.java | 2 +- .../client}/utils/video/MediaCodecManager.java | 3 ++- .../o3dr/android/client/utils/video/NaluChunk.java | 2 +- .../client/utils/video/NaluChunkAssembler.java | 14 ++++++++------ .../android/utils/video/StreamRecorder.java | 2 ++ .../services/android/utils/video/VideoManager.java | 2 ++ 6 files changed, 16 insertions(+), 9 deletions(-) rename {ServiceApp/src/org/droidplanner/services/android => ClientLib/src/main/java/com/o3dr/android/client}/utils/video/DecoderListener.java (75%) rename {ServiceApp/src/org/droidplanner/services/android => ClientLib/src/main/java/com/o3dr/android/client}/utils/video/MediaCodecManager.java (98%) rename ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunk.java => ClientLib/src/main/java/com/o3dr/android/client/utils/video/NaluChunk.java (93%) rename ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunkAssembler.java => ClientLib/src/main/java/com/o3dr/android/client/utils/video/NaluChunkAssembler.java (93%) diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/DecoderListener.java b/ClientLib/src/main/java/com/o3dr/android/client/utils/video/DecoderListener.java similarity index 75% rename from ServiceApp/src/org/droidplanner/services/android/utils/video/DecoderListener.java rename to ClientLib/src/main/java/com/o3dr/android/client/utils/video/DecoderListener.java index 05b3a0bd6d..9178ad7ffd 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/DecoderListener.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/utils/video/DecoderListener.java @@ -1,4 +1,4 @@ -package org.droidplanner.services.android.utils.video; +package com.o3dr.android.client.utils.video; /** * Created by fhuya on 12/4/14. diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/MediaCodecManager.java b/ClientLib/src/main/java/com/o3dr/android/client/utils/video/MediaCodecManager.java similarity index 98% rename from ServiceApp/src/org/droidplanner/services/android/utils/video/MediaCodecManager.java rename to ClientLib/src/main/java/com/o3dr/android/client/utils/video/MediaCodecManager.java index 765aa0e53d..96b4eb0cf0 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/MediaCodecManager.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/utils/video/MediaCodecManager.java @@ -1,4 +1,4 @@ -package org.droidplanner.services.android.utils.video; +package com.o3dr.android.client.utils.video; import android.annotation.TargetApi; import android.media.MediaCodec; @@ -14,6 +14,7 @@ import java.util.concurrent.atomic.AtomicReference; /** + * Decodes video stream bytes for playing back in a Surface. * Created by Fredia Huya-Kouadio on 2/19/15. */ @TargetApi(Build.VERSION_CODES.JELLY_BEAN_MR2) diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunk.java b/ClientLib/src/main/java/com/o3dr/android/client/utils/video/NaluChunk.java similarity index 93% rename from ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunk.java rename to ClientLib/src/main/java/com/o3dr/android/client/utils/video/NaluChunk.java index bb0a712038..d77f313545 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunk.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/utils/video/NaluChunk.java @@ -1,4 +1,4 @@ -package org.droidplanner.services.android.utils.video; +package com.o3dr.android.client.utils.video; import java.nio.ByteBuffer; diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunkAssembler.java b/ClientLib/src/main/java/com/o3dr/android/client/utils/video/NaluChunkAssembler.java similarity index 93% rename from ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunkAssembler.java rename to ClientLib/src/main/java/com/o3dr/android/client/utils/video/NaluChunkAssembler.java index e9b842cbd3..c5ee961cb1 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/NALUChunkAssembler.java +++ b/ClientLib/src/main/java/com/o3dr/android/client/utils/video/NaluChunkAssembler.java @@ -1,12 +1,12 @@ -package org.droidplanner.services.android.utils.video; +package com.o3dr.android.client.utils.video; import android.annotation.TargetApi; import android.media.MediaCodec; import android.os.Build; +import android.util.Log; import java.nio.ByteBuffer; - -import timber.log.Timber; +import java.util.Locale; /** * Created by Fredia Huya-Kouadio on 6/1/15. @@ -14,6 +14,8 @@ @TargetApi(Build.VERSION_CODES.JELLY_BEAN_MR2) class NaluChunkAssembler { + private static final String TAG = NaluChunkAssembler.class.getSimpleName(); + private final NaluChunk assembledNaluChunk; private final NaluChunk paramsNaluChunk; private final NaluChunk eosNaluChunk; @@ -76,7 +78,7 @@ NaluChunk assembleNALUChunk(byte[] buffer, int bufferLength) { final byte nalHeaderByte = buffer[12]; final int forbiddenBit = (nalHeaderByte & 0x80) >> 7; if (forbiddenBit != 0) { - Timber.w("Forbidden bit is set, indicating possible errors."); + Log.w(TAG, "Forbidden bit is set, indicating possible errors."); return null; } @@ -89,7 +91,7 @@ NaluChunk assembleNALUChunk(byte[] buffer, int bufferLength) { final int sequenceNumber = ((buffer[2] & 0xff) << 8) | (buffer[3] & 0xff); final int nalType = nalHeaderByte & 0x1f; if (nalType <= 0) { - Timber.d("Undefined nal type: " + nalType); + Log.d(TAG, "Undefined nal type: " + nalType); return null; } @@ -97,7 +99,7 @@ NaluChunk assembleNALUChunk(byte[] buffer, int bufferLength) { if(prevSeq != -1){ final int expectedSeq = prevSeq + 1; if(sequenceNumber != expectedSeq){ - Timber.v("Sequence number is out of order: %d != %d", expectedSeq, sequenceNumber); + Log.v(TAG, String.format(Locale.US, "Sequence number is out of order: %d != %d", expectedSeq, sequenceNumber)); } } prevSeq = sequenceNumber; diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamRecorder.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamRecorder.java index 936c7692f6..08de14fe4e 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamRecorder.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/video/StreamRecorder.java @@ -11,6 +11,8 @@ import com.googlecode.mp4parser.authoring.Movie; import com.googlecode.mp4parser.authoring.builder.DefaultMp4Builder; import com.googlecode.mp4parser.authoring.tracks.h264.H264TrackImpl; +import com.o3dr.android.client.utils.video.MediaCodecManager; +import com.o3dr.android.client.utils.video.NaluChunk; import java.io.BufferedOutputStream; import java.io.File; diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java b/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java index 193d5c2943..7856e71329 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/video/VideoManager.java @@ -11,6 +11,8 @@ import com.o3dr.android.client.utils.connection.AbstractIpConnection; import com.o3dr.android.client.utils.connection.IpConnectionListener; import com.o3dr.android.client.utils.connection.UdpConnection; +import com.o3dr.android.client.utils.video.DecoderListener; +import com.o3dr.android.client.utils.video.MediaCodecManager; import com.o3dr.services.android.lib.drone.action.CameraActions; import com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError; import com.o3dr.services.android.lib.model.ICommandListener; From 4078eea7ca423021f198b91383a308219a4323d6 Mon Sep 17 00:00:00 2001 From: fredia Date: Wed, 24 Feb 2016 10:35:35 -0800 Subject: [PATCH 26/37] Remove redundant media codec logic --- .../o3dr/sample/hellodrone/MainActivity.java | 4 +- .../video/utils/DecoderListener.java | 14 - .../video/utils/MediaCodecManager.java | 271 ------------------ .../hellodrone/video/utils/NALUChunk.java | 30 -- .../video/utils/NALUChunkAssembler.java | 199 ------------- 5 files changed, 2 insertions(+), 516 deletions(-) delete mode 100644 samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/DecoderListener.java delete mode 100644 samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/MediaCodecManager.java delete mode 100644 samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/NALUChunk.java delete mode 100644 samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/NALUChunkAssembler.java diff --git a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java index 830643a00d..738506e70a 100644 --- a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java +++ b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/MainActivity.java @@ -25,8 +25,8 @@ import com.o3dr.android.client.apis.VehicleApi; import com.o3dr.android.client.interfaces.DroneListener; import com.o3dr.android.client.interfaces.TowerListener; -import com.o3dr.sample.hellodrone.video.utils.DecoderListener; -import com.o3dr.sample.hellodrone.video.utils.MediaCodecManager; +import com.o3dr.android.client.utils.video.DecoderListener; +import com.o3dr.android.client.utils.video.MediaCodecManager; import com.o3dr.services.android.lib.coordinate.LatLong; import com.o3dr.services.android.lib.coordinate.LatLongAlt; import com.o3dr.services.android.lib.drone.attribute.AttributeEvent; diff --git a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/DecoderListener.java b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/DecoderListener.java deleted file mode 100644 index 0875917861..0000000000 --- a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/DecoderListener.java +++ /dev/null @@ -1,14 +0,0 @@ -package com.o3dr.sample.hellodrone.video.utils; - -/** - * Created by fhuya on 12/4/14. - */ -public interface DecoderListener { - - void onDecodingStarted(); - - void onDecodingError(); - - void onDecodingEnded(); - -} diff --git a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/MediaCodecManager.java b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/MediaCodecManager.java deleted file mode 100644 index 2a152f86ba..0000000000 --- a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/MediaCodecManager.java +++ /dev/null @@ -1,271 +0,0 @@ -package com.o3dr.sample.hellodrone.video.utils; - -import android.annotation.TargetApi; -import android.media.MediaCodec; -import android.media.MediaFormat; -import android.os.Build; -import android.os.Handler; -import android.util.Log; -import android.view.Surface; - -import java.io.IOException; -import java.nio.ByteBuffer; -import java.util.concurrent.atomic.AtomicBoolean; -import java.util.concurrent.atomic.AtomicReference; - -/** - * Decodes video stream bytes for playing back in a Surface. - */ -@TargetApi(Build.VERSION_CODES.JELLY_BEAN_MR2) -public class MediaCodecManager { - private static final String TAG = MediaCodecManager.class.getSimpleName(); - - private static final String MIME_TYPE = "video/avc"; - public static final int DEFAULT_VIDEO_WIDTH = 1920; - public static final int DEFAULT_VIDEO_HEIGHT = 1080; - - private final Runnable stopSafely = new Runnable() { - @Override - public void run() { - processInputData.set(false); - sendCompletionFlag.set(false); - naluChunkAssembler.reset(); - - if (dequeueRunner != null && dequeueRunner.isAlive()) { - Log.d(TAG, "Interrupting dequeue runner thread."); - dequeueRunner.interrupt(); - } - dequeueRunner = null; - - final MediaCodec mediaCodec = mediaCodecRef.get(); - if (mediaCodec != null) { - try { - mediaCodec.stop(); - }catch(IllegalStateException e){ - Log.e(TAG, "Error while stopping media codec.", e); - } - mediaCodec.release(); - mediaCodecRef.set(null); - } - - surfaceRef.set(null); - - isDecoding.set(false); - handler.post(decodingEndedNotification); - } - }; - - private final Runnable decodingStartedNotification = new Runnable() { - @Override - public void run() { - final DecoderListener listener = decoderListenerRef.get(); - if (listener != null) - listener.onDecodingStarted(); - } - }; - - private final Runnable decodingErrorNotification = new Runnable() { - @Override - public void run() { - final DecoderListener listener = decoderListenerRef.get(); - if (listener != null) - listener.onDecodingError(); - } - }; - - private final Runnable decodingEndedNotification = new Runnable() { - @Override - public void run() { - final DecoderListener listener = decoderListenerRef.get(); - if (listener != null) - listener.onDecodingEnded(); - } - }; - - private final AtomicBoolean decodedFirstFrame = new AtomicBoolean(false); - - private final AtomicBoolean isDecoding = new AtomicBoolean(false); - private final AtomicBoolean processInputData = new AtomicBoolean(false); - private final AtomicBoolean sendCompletionFlag = new AtomicBoolean(false); - private final AtomicReference surfaceRef = new AtomicReference<>(); - private final AtomicReference mediaCodecRef = new AtomicReference<>(); - private final AtomicReference decoderListenerRef = new AtomicReference<>(); - private final NALUChunkAssembler naluChunkAssembler; - - private final Handler handler; - - private DequeueCodec dequeueRunner; - - public MediaCodecManager(Handler handler) { - this.handler = handler; - this.naluChunkAssembler = new NALUChunkAssembler(); - } - - public void startDecoding(Surface surface, DecoderListener listener) throws IOException { - if (surface == null) - throw new IllegalStateException("Surface argument must be non-null."); - - if (isDecoding.compareAndSet(false, true)) { - Log.i(TAG, "Starting decoding..."); - this.naluChunkAssembler.reset(); - - this.decoderListenerRef.set(listener); - - final MediaFormat mediaFormat = MediaFormat.createVideoFormat(MIME_TYPE, DEFAULT_VIDEO_WIDTH, - DEFAULT_VIDEO_HEIGHT); - - final MediaCodec mediaCodec = MediaCodec.createDecoderByType(MIME_TYPE); - mediaCodec.configure(mediaFormat, surface, null, 0); - mediaCodec.start(); - - surfaceRef.set(surface); - mediaCodecRef.set(mediaCodec); - processInputData.set(true); - - dequeueRunner = new DequeueCodec(); - dequeueRunner.start(); - } - } - - public void stopDecoding(DecoderListener listener) { - Log.i(TAG, "Stopping input data processing..."); - - this.decoderListenerRef.set(listener); - if (!isDecoding.get()) { - if (listener != null) { - notifyDecodingEnded(); - } - } - else { - if (decodedFirstFrame.get()) { - if (processInputData.compareAndSet(true, false)) { - sendCompletionFlag.set(!processNALUChunk(naluChunkAssembler.getEndOfStream())); - } - } - else { - handler.post(stopSafely); - } - } - } - - public void onInputDataReceived(byte[] data, int dataSize) { - if (isDecoding.get()) { - if (processInputData.get()) { - //Process the received buffer - NALUChunk naluChunk = naluChunkAssembler.assembleNALUChunk(data, dataSize); - if (naluChunk != null) - processNALUChunk(naluChunk); - } else { - if (sendCompletionFlag.get()) { - Log.d(TAG, "Sending end of stream data."); - sendCompletionFlag.set(!processNALUChunk(naluChunkAssembler.getEndOfStream())); - } - } - } - } - - private boolean processNALUChunk(NALUChunk naluChunk) { - if (naluChunk == null) - return false; - - final MediaCodec mediaCodec = mediaCodecRef.get(); - if (mediaCodec == null) - return false; - - try { - final int index = mediaCodec.dequeueInputBuffer(-1); - if (index >= 0) { - ByteBuffer inputBuffer; - if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.LOLLIPOP) { - inputBuffer = mediaCodec.getInputBuffer(index); - } else { - inputBuffer = mediaCodec.getInputBuffers()[index]; - } - - if (inputBuffer == null) - return false; - - inputBuffer.clear(); - int totalLength = 0; - - int payloadCount = naluChunk.payloads.length; - for (int i = 0; i < payloadCount; i++) { - ByteBuffer payload = naluChunk.payloads[i]; - - if (payload.capacity() == 0) - continue; - - inputBuffer.order(payload.order()); - final int dataLength = payload.position(); - byte[] payloadData = payload.array(); - inputBuffer.put(payloadData, 0, dataLength); - - totalLength += dataLength; - } - - mediaCodec.queueInputBuffer(index, 0, totalLength, naluChunk.presentationTime, naluChunk.flags); - } - } catch (IllegalStateException e) { - Log.e(TAG, e.getMessage(), e); - return false; - } - - return true; - } - - private void notifyDecodingStarted() { - handler.post(decodingStartedNotification); - } - - private void notifyDecodingError() { - handler.post(decodingErrorNotification); - } - - private void notifyDecodingEnded() { - handler.post(stopSafely); - } - - private class DequeueCodec extends Thread { - @Override - public void run() { - final MediaCodec mediaCodec = mediaCodecRef.get(); - if (mediaCodec == null) - throw new IllegalStateException("Start decoding hasn't been called yet."); - - Log.i(TAG, "Starting dequeue codec runner."); - - final MediaCodec.BufferInfo info = new MediaCodec.BufferInfo(); - decodedFirstFrame.set(false); - boolean doRender; - boolean continueDequeue = true; - try { - while (continueDequeue) { - final int index = mediaCodec.dequeueOutputBuffer(info, -1); - if (index >= 0) { - doRender = info.size != 0; - mediaCodec.releaseOutputBuffer(index, doRender); - - if (decodedFirstFrame.compareAndSet(false, true)) { - notifyDecodingStarted(); - Log.i(TAG, "Received first decoded frame of size " + info.size); - } - - continueDequeue = (info.flags & MediaCodec.BUFFER_FLAG_END_OF_STREAM) == 0; - if (!continueDequeue) { - Log.i(TAG, "Received end of stream flag."); - } - } - } - } catch (IllegalStateException e) { - if(!isInterrupted()) { - Log.e(TAG, "Decoding error!", e); - notifyDecodingError(); - } - } finally { - if (!isInterrupted()) - notifyDecodingEnded(); - Log.i(TAG, "Stopping dequeue codec runner."); - } - } - } -} diff --git a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/NALUChunk.java b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/NALUChunk.java deleted file mode 100644 index 73841e997a..0000000000 --- a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/NALUChunk.java +++ /dev/null @@ -1,30 +0,0 @@ -package com.o3dr.sample.hellodrone.video.utils; - -import java.nio.ByteBuffer; - -/** - * Created by fhuya on 12/4/14. - */ -public class NALUChunk { - public static final byte[] START_CODE = {0, 0, 0, 1}; - public static final int SPS_NAL_TYPE = 7; - public static final int PPS_NAL_TYPE = 8; - - public final ByteBuffer[] payloads; - - public NALUChunk(int payloadCount, int payloadSize, byte[] payloadInitData) { - this.payloads = new ByteBuffer[payloadCount]; - for (int i = 0; i < payloadCount; i++) { - this.payloads[i] = ByteBuffer.allocate(payloadSize); - if (payloadInitData != null) { - this.payloads[i].put(payloadInitData); - this.payloads[i].mark(); - } - } - } - - public int type; - public int sequenceNumber; - public int flags; - public long presentationTime; -} diff --git a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/NALUChunkAssembler.java b/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/NALUChunkAssembler.java deleted file mode 100644 index d2c9b81fcd..0000000000 --- a/samples/StarterApp/src/main/java/com/o3dr/sample/hellodrone/video/utils/NALUChunkAssembler.java +++ /dev/null @@ -1,199 +0,0 @@ -package com.o3dr.sample.hellodrone.video.utils; - -import android.annotation.TargetApi; -import android.media.MediaCodec; -import android.os.Build; -import android.util.Log; - -import java.nio.ByteBuffer; - -/** - * TODO - */ -@TargetApi(Build.VERSION_CODES.JELLY_BEAN_MR2) -public class NALUChunkAssembler { - private static final String TAG = NALUChunkAssembler.class.getSimpleName(); - - private final NALUChunk assembledNaluChunk; - private final NALUChunk paramsNaluChunk; - private final NALUChunk eosNaluChunk; - - /** - * Stores the sps data so it can be concatenate with the pps data. - */ - private final static int SPS_BUFFER_INDEX = 0; - private boolean isSpsSet = false; - - /** - * Stores the pps data so it can be concatenate with the sps data. - */ - private final static int PPS_BUFFER_INDEX = 1; - private boolean isPpsSet = false; - - NALUChunkAssembler() { - this.assembledNaluChunk = new NALUChunk(1, 1024 * 1024, NALUChunk.START_CODE); - - this.paramsNaluChunk = new NALUChunk(2, 256, NALUChunk.START_CODE); - this.paramsNaluChunk.type = 78; - this.paramsNaluChunk.flags = MediaCodec.BUFFER_FLAG_CODEC_CONFIG; - - this.eosNaluChunk = new NALUChunk(1, 0, null); - this.eosNaluChunk.flags = MediaCodec.BUFFER_FLAG_END_OF_STREAM; - } - - void reset() { - isSpsSet = false; - isPpsSet = false; - - this.assembledNaluChunk.payloads[0].reset(); - - this.paramsNaluChunk.payloads[0].reset(); - this.paramsNaluChunk.payloads[1].reset(); - } - - private boolean areParametersSet() { - return isSpsSet && isPpsSet; - } - - NALUChunk getEndOfStream(){ - return eosNaluChunk; - } - - private int prevSeq = -1; - private int naluCounter = 0; - private final static long DELTA_PRESENTATION_TIME = 42000L; - - NALUChunk getParametersSet(){ - if(areParametersSet()) - return paramsNaluChunk; - - return null; - } - - NALUChunk assembleNALUChunk(byte[] buffer, int bufferLength) { - - //The first 12 bytes are the rtp header. - final byte nalHeaderByte = buffer[12]; - final int forbiddenBit = (nalHeaderByte & 0x80) >> 7; - if (forbiddenBit != 0) { - Log.w(TAG, "Forbidden bit is set, indicating possible errors."); - return null; - } - - long rtpTimestamp = 0; - rtpTimestamp |= (buffer[4] & 0xffl) << 24; - rtpTimestamp |= (buffer[5] & 0xffl) << 16; - rtpTimestamp |= (buffer[6] & 0xffl) << 8; - rtpTimestamp |= (buffer[7] & 0xffl); - - final int sequenceNumber = ((buffer[2] & 0xff) << 8) | (buffer[3] & 0xff); - final int nalType = nalHeaderByte & 0x1f; - if (nalType <= 0) { - Log.d(TAG, "Undefined nal type: " + nalType); - return null; - } - - //DEBUG LOGIC - if(prevSeq != -1){ - final int expectedSeq = prevSeq + 1; - if (sequenceNumber != expectedSeq){ - Log.v(TAG, "Sequence number is out of order: " + expectedSeq + " != " + sequenceNumber); - } - } - prevSeq = sequenceNumber; - //DEBUG LOGIC - - if (nalType <= 23) { - //Single nal unit packet. - final int payloadOffset = 12; - final int payloadLength = bufferLength - payloadOffset; - switch (nalType) { - case 7: //SPS parameters set. - case 8: //PPS parameters set. - { - ByteBuffer naluData; - if (nalType == NALUChunk.SPS_NAL_TYPE) { - naluData = paramsNaluChunk.payloads[SPS_BUFFER_INDEX]; - isSpsSet = true; - } else { - naluData = paramsNaluChunk.payloads[PPS_BUFFER_INDEX]; - isPpsSet = true; - } - - naluData.reset(); - naluData.put(buffer, payloadOffset, payloadLength); - - if (areParametersSet()) { - paramsNaluChunk.sequenceNumber = sequenceNumber; - paramsNaluChunk.presentationTime = 0; - return paramsNaluChunk; - } - - return null; - } - - default: - if (!areParametersSet()) - return null; - - ByteBuffer assembledNaluBuffer = assembledNaluChunk.payloads[0]; - assembledNaluBuffer.reset(); - assembledNaluBuffer.put(buffer, payloadOffset, payloadLength); - - assembledNaluChunk.type = nalType; - assembledNaluChunk.sequenceNumber = sequenceNumber; - assembledNaluChunk.flags = 0; - assembledNaluChunk.presentationTime = naluCounter++ * DELTA_PRESENTATION_TIME; - return assembledNaluChunk; - } - } - - if (nalType == 28) { - //Fragmentation unit - if (!areParametersSet()) - return null; - - final int payloadOffset = 14; - final int payloadLength = bufferLength - payloadOffset; - - final int fuIndicatorByte = nalHeaderByte; - final int fuHeaderByte = buffer[13]; - final int fuNalType = fuHeaderByte & 0x1f; - final int startBit = (fuHeaderByte & 0x80) >> 7; - final int endBit = (fuHeaderByte & 0x40) >> 6; - - if (startBit == 1) { - ByteBuffer assembledNaluBuffer = assembledNaluChunk.payloads[0]; - assembledNaluBuffer.reset(); - - assembledNaluBuffer.put((byte) ((fuIndicatorByte & 0xe0) | fuNalType)); - assembledNaluBuffer.put(buffer, payloadOffset, payloadLength); - - boolean isConfig = fuNalType == 7 || fuNalType == 8; - - assembledNaluChunk.sequenceNumber = sequenceNumber; - assembledNaluChunk.type = fuNalType; - assembledNaluChunk.flags = isConfig ? MediaCodec.BUFFER_FLAG_CODEC_CONFIG : 0; -// assembledNaluChunk.presentationTime = rtpTimestamp; - return null; - } else { - if (sequenceNumber - 1 != assembledNaluChunk.sequenceNumber) { - return null; - } - - ByteBuffer assembledNaluBuffer = assembledNaluChunk.payloads[0]; - assembledNaluBuffer.put(buffer, payloadOffset, payloadLength); - assembledNaluChunk.sequenceNumber = sequenceNumber; - - if (endBit == 1) { - assembledNaluChunk.presentationTime = naluCounter++ * DELTA_PRESENTATION_TIME; - return assembledNaluChunk; - } else { - return null; - } - } - } - - return null; - } -} From 1330074fa109b25baed098ff7ab7e6c5cb66c1bc Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Fri, 26 Feb 2016 14:32:30 -0800 Subject: [PATCH 27/37] Always save wifi ssid when connected --- .../android/utils/connection/WifiConnectionHandler.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java index 59debd0d37..c1488398a5 100644 --- a/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java +++ b/ServiceApp/src/org/droidplanner/services/android/utils/connection/WifiConnectionHandler.java @@ -438,6 +438,8 @@ private void setDefaultNetworkIfNecessary(String wifiSsid) { final String trimmedSsid = trimWifiSsid(wifiSsid); if (!trimmedSsid.equals(connectedWifi.get())) { + connectedWifi.set(trimmedSsid); + if (isConnected(wifiSsid)) { notifyWifiConnected(wifiSsid); return; @@ -455,7 +457,6 @@ private void setDefaultNetworkIfNecessary(String wifiSsid) { } else { notifyWifiConnected(trimmedSsid); } - connectedWifi.set(trimmedSsid); } } From 527566312885d1f9820625045219a9bbe52d0622 Mon Sep 17 00:00:00 2001 From: fredia Date: Fri, 26 Feb 2016 16:32:33 -0800 Subject: [PATCH 28/37] Bump version build to the third beta --- ServiceApp/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ServiceApp/build.gradle b/ServiceApp/build.gradle index d394b0c25b..ec6d6f5970 100644 --- a/ServiceApp/build.gradle +++ b/ServiceApp/build.gradle @@ -40,7 +40,7 @@ dependencies { def versionMajor = 1 def versionMinor = 5 def versionPatch = 2 -def versionBuild = 1 //bump for dogfood builds, public betas, etc. +def versionBuild = 2 //bump for dogfood builds, public betas, etc. def versionPrefix = "3DR Services v" //Log levels values From b513c3c4ce32129018e81b6524ad5c6f19fd766c Mon Sep 17 00:00:00 2001 From: fredia Date: Mon, 29 Feb 2016 16:25:29 -0800 Subject: [PATCH 29/37] Override default equals, hashCode and toString methods for the mission items --- .../lib/drone/mission/item/MissionItem.java | 15 ++++++ .../mission/item/command/CameraTrigger.java | 28 +++++++++++ .../mission/item/command/ChangeSpeed.java | 28 +++++++++++ .../drone/mission/item/command/DoJump.java | 29 +++++++++++ .../mission/item/command/EpmGripper.java | 26 ++++++++++ .../drone/mission/item/command/ResetROI.java | 5 ++ .../mission/item/command/ReturnToLaunch.java | 28 +++++++++++ .../drone/mission/item/command/SetRelay.java | 29 +++++++++++ .../drone/mission/item/command/SetServo.java | 29 +++++++++++ .../drone/mission/item/command/Takeoff.java | 32 ++++++++++++ .../mission/item/command/YawCondition.java | 35 +++++++++++++ .../mission/item/complex/SplineSurvey.java | 5 ++ .../item/complex/StructureScanner.java | 46 +++++++++++++++++ .../drone/mission/item/complex/Survey.java | 49 +++++++++++++++++++ .../mission/item/spatial/BaseSpatialItem.java | 26 ++++++++++ .../drone/mission/item/spatial/Circle.java | 23 +++++++++ .../mission/item/spatial/DoLandStart.java | 5 ++ .../lib/drone/mission/item/spatial/Land.java | 5 ++ .../item/spatial/RegionOfInterest.java | 5 ++ .../mission/item/spatial/SplineWaypoint.java | 29 +++++++++++ .../drone/mission/item/spatial/Waypoint.java | 44 +++++++++++++++++ ClientLib/src/main/res/values/version.xml | 2 +- 22 files changed, 522 insertions(+), 1 deletion(-) diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/MissionItem.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/MissionItem.java index 00b12e3568..3960efdcd1 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/MissionItem.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/MissionItem.java @@ -52,4 +52,19 @@ protected MissionItem(Parcel in){ @Override public abstract MissionItem clone(); + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof MissionItem)) return false; + + MissionItem that = (MissionItem) o; + + return type == that.type; + + } + + @Override + public int hashCode() { + return type != null ? type.hashCode() : 0; + } } diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/CameraTrigger.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/CameraTrigger.java index 7c0bd2788b..bdb4c6eb0e 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/CameraTrigger.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/CameraTrigger.java @@ -29,6 +29,34 @@ public void setTriggerDistance(double triggerDistance) { this.triggerDistance = triggerDistance; } + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof CameraTrigger)) return false; + if (!super.equals(o)) return false; + + CameraTrigger that = (CameraTrigger) o; + + return Double.compare(that.triggerDistance, triggerDistance) == 0; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + temp = Double.doubleToLongBits(triggerDistance); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + return result; + } + + @Override + public String toString() { + return "CameraTrigger{" + + "triggerDistance=" + triggerDistance + + '}'; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ChangeSpeed.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ChangeSpeed.java index 4f271f292b..f119571d85 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ChangeSpeed.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ChangeSpeed.java @@ -29,6 +29,34 @@ public void setSpeed(double speed) { this.speed = speed; } + @Override + public String toString() { + return "ChangeSpeed{" + + "speed=" + speed + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof ChangeSpeed)) return false; + if (!super.equals(o)) return false; + + ChangeSpeed that = (ChangeSpeed) o; + + return Double.compare(that.speed, speed) == 0; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + temp = Double.doubleToLongBits(speed); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/DoJump.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/DoJump.java index 1e7ccc5759..fa0d8bea3a 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/DoJump.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/DoJump.java @@ -28,6 +28,35 @@ protected DoJump(Parcel in) { repeatCount = in.readInt(); } + @Override + public String toString() { + return "DoJump{" + + "repeatCount=" + repeatCount + + ", waypoint=" + waypoint + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof DoJump)) return false; + if (!super.equals(o)) return false; + + DoJump doJump = (DoJump) o; + + if (waypoint != doJump.waypoint) return false; + return repeatCount == doJump.repeatCount; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + result = 31 * result + waypoint; + result = 31 * result + repeatCount; + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/EpmGripper.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/EpmGripper.java index b4009fc8a3..9b7c68ce63 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/EpmGripper.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/EpmGripper.java @@ -29,6 +29,32 @@ public void setRelease(boolean release) { this.release = release; } + @Override + public String toString() { + return "EpmGripper{" + + "release=" + release + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof EpmGripper)) return false; + if (!super.equals(o)) return false; + + EpmGripper that = (EpmGripper) o; + + return release == that.release; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + result = 31 * result + (release ? 1 : 0); + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ResetROI.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ResetROI.java index 3f617cfeac..4ffe4b5274 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ResetROI.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ResetROI.java @@ -25,6 +25,11 @@ public MissionItem clone() { return new ResetROI(this); } + @Override + public String toString() { + return "ResetROI{}"; + } + @Override public int describeContents() { return 0; diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ReturnToLaunch.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ReturnToLaunch.java index df9bf4dc9e..ce81de4825 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ReturnToLaunch.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/ReturnToLaunch.java @@ -29,6 +29,34 @@ public void setReturnAltitude(double returnAltitude) { this.returnAltitude = returnAltitude; } + @Override + public String toString() { + return "ReturnToLaunch{" + + "returnAltitude=" + returnAltitude + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof ReturnToLaunch)) return false; + if (!super.equals(o)) return false; + + ReturnToLaunch that = (ReturnToLaunch) o; + + return Double.compare(that.returnAltitude, returnAltitude) == 0; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + temp = Double.doubleToLongBits(returnAltitude); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/SetRelay.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/SetRelay.java index db42b85644..fd711d30c5 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/SetRelay.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/SetRelay.java @@ -58,6 +58,35 @@ public MissionItem clone() { return new SetRelay(this); } + @Override + public String toString() { + return "SetRelay{" + + "enabled=" + enabled + + ", relayNumber=" + relayNumber + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof SetRelay)) return false; + if (!super.equals(o)) return false; + + SetRelay setRelay = (SetRelay) o; + + if (relayNumber != setRelay.relayNumber) return false; + return enabled == setRelay.enabled; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + result = 31 * result + relayNumber; + result = 31 * result + (enabled ? 1 : 0); + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/SetServo.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/SetServo.java index d11ede9c0b..8b0debe0d8 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/SetServo.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/SetServo.java @@ -52,6 +52,35 @@ public void setChannel(int channel) { this.channel = channel; } + @Override + public String toString() { + return "SetServo{" + + "channel=" + channel + + ", pwm=" + pwm + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof SetServo)) return false; + if (!super.equals(o)) return false; + + SetServo setServo = (SetServo) o; + + if (pwm != setServo.pwm) return false; + return channel == setServo.channel; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + result = 31 * result + pwm; + result = 31 * result + channel; + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/Takeoff.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/Takeoff.java index 1b1d9df9a2..1d6d0ab60f 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/Takeoff.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/Takeoff.java @@ -51,6 +51,38 @@ public void setTakeoffPitch(double takeoffPitch) { this.takeoffPitch = takeoffPitch; } + @Override + public String toString() { + return "Takeoff{" + + "takeoffAltitude=" + takeoffAltitude + + ", takeoffPitch=" + takeoffPitch + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof Takeoff)) return false; + if (!super.equals(o)) return false; + + Takeoff takeoff = (Takeoff) o; + + if (Double.compare(takeoff.takeoffAltitude, takeoffAltitude) != 0) return false; + return Double.compare(takeoff.takeoffPitch, takeoffPitch) == 0; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + temp = Double.doubleToLongBits(takeoffAltitude); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(takeoffPitch); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/YawCondition.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/YawCondition.java index 12494f6a4c..e73e865c83 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/YawCondition.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/command/YawCondition.java @@ -49,6 +49,41 @@ public void setRelative(boolean isRelative) { this.isRelative = isRelative; } + @Override + public String toString() { + return "YawCondition{" + + "angle=" + angle + + ", angularSpeed=" + angularSpeed + + ", isRelative=" + isRelative + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof YawCondition)) return false; + if (!super.equals(o)) return false; + + YawCondition that = (YawCondition) o; + + if (Double.compare(that.angle, angle) != 0) return false; + if (Double.compare(that.angularSpeed, angularSpeed) != 0) return false; + return isRelative == that.isRelative; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + temp = Double.doubleToLongBits(angle); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(angularSpeed); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + result = 31 * result + (isRelative ? 1 : 0); + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/SplineSurvey.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/SplineSurvey.java index f8594f1021..15c4a35653 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/SplineSurvey.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/SplineSurvey.java @@ -26,6 +26,11 @@ private SplineSurvey(Parcel in) { super(in); } + @Override + public String toString() { + return "SplineSurvey{" + super.toString() + "}"; + } + @Override public MissionItem clone() { return new SplineSurvey(this); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/StructureScanner.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/StructureScanner.java index c9b67de55a..1c8eba77db 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/StructureScanner.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/StructureScanner.java @@ -119,6 +119,52 @@ private StructureScanner(Parcel in) { in.readTypedList(path, LatLong.CREATOR); } + @Override + public String toString() { + return "StructureScanner{" + + "crossHatch=" + crossHatch + + ", radius=" + radius + + ", heightStep=" + heightStep + + ", stepsCount=" + stepsCount + + ", surveyDetail=" + surveyDetail + + ", path=" + path + + ", " + super.toString() + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof StructureScanner)) return false; + if (!super.equals(o)) return false; + + StructureScanner that = (StructureScanner) o; + + if (Double.compare(that.radius, radius) != 0) return false; + if (Double.compare(that.heightStep, heightStep) != 0) return false; + if (stepsCount != that.stepsCount) return false; + if (crossHatch != that.crossHatch) return false; + if (surveyDetail != null ? !surveyDetail.equals(that.surveyDetail) : that.surveyDetail != null) + return false; + return !(path != null ? !path.equals(that.path) : that.path != null); + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + temp = Double.doubleToLongBits(radius); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(heightStep); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + result = 31 * result + stepsCount; + result = 31 * result + (crossHatch ? 1 : 0); + result = 31 * result + (surveyDetail != null ? surveyDetail.hashCode() : 0); + result = 31 * result + (path != null ? path.hashCode() : 0); + return result; + } + @Override public MissionItem clone() { return new StructureScanner(this); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/Survey.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/Survey.java index f55a044ba4..f0ae8c7fd5 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/Survey.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/complex/Survey.java @@ -138,6 +138,55 @@ public int getCameraCount() { return getCameraLocations().size(); } + @Override + public String toString() { + return "Survey{" + + "cameraLocations=" + cameraLocations + + ", surveyDetail=" + surveyDetail + + ", polygonArea=" + polygonArea + + ", polygonPoints=" + polygonPoints + + ", gridPoints=" + gridPoints + + ", isValid=" + isValid + + ", startCameraBeforeFirstWaypoint=" + startCameraBeforeFirstWaypoint + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof Survey)) return false; + if (!super.equals(o)) return false; + + Survey survey = (Survey) o; + + if (Double.compare(survey.polygonArea, polygonArea) != 0) return false; + if (isValid != survey.isValid) return false; + if (startCameraBeforeFirstWaypoint != survey.startCameraBeforeFirstWaypoint) return false; + if (surveyDetail != null ? !surveyDetail.equals(survey.surveyDetail) : survey.surveyDetail != null) + return false; + if (polygonPoints != null ? !polygonPoints.equals(survey.polygonPoints) : survey.polygonPoints != null) + return false; + if (gridPoints != null ? !gridPoints.equals(survey.gridPoints) : survey.gridPoints != null) + return false; + return !(cameraLocations != null ? !cameraLocations.equals(survey.cameraLocations) : survey.cameraLocations != null); + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + result = 31 * result + (surveyDetail != null ? surveyDetail.hashCode() : 0); + temp = Double.doubleToLongBits(polygonArea); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + result = 31 * result + (polygonPoints != null ? polygonPoints.hashCode() : 0); + result = 31 * result + (gridPoints != null ? gridPoints.hashCode() : 0); + result = 31 * result + (cameraLocations != null ? cameraLocations.hashCode() : 0); + result = 31 * result + (isValid ? 1 : 0); + result = 31 * result + (startCameraBeforeFirstWaypoint ? 1 : 0); + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/BaseSpatialItem.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/BaseSpatialItem.java index 66046b61f3..a26045f4fe 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/BaseSpatialItem.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/BaseSpatialItem.java @@ -22,6 +22,32 @@ protected BaseSpatialItem(BaseSpatialItem copy){ coordinate = copy.coordinate == null ? null : new LatLongAlt(copy.coordinate); } + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof BaseSpatialItem)) return false; + if (!super.equals(o)) return false; + + BaseSpatialItem that = (BaseSpatialItem) o; + + return !(coordinate != null ? !coordinate.equals(that.coordinate) : that.coordinate != null); + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + result = 31 * result + (coordinate != null ? coordinate.hashCode() : 0); + return result; + } + + @Override + public String toString() { + return "BaseSpatialItem{" + + "coordinate=" + coordinate + + '}'; + } + @Override public LatLongAlt getCoordinate() { return coordinate; diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Circle.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Circle.java index 4f4f4b3548..9da7d5c04c 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Circle.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Circle.java @@ -40,6 +40,29 @@ public void setTurns(int turns) { this.turns = turns; } + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof Circle)) return false; + if (!super.equals(o)) return false; + + Circle circle = (Circle) o; + + if (Double.compare(circle.radius, radius) != 0) return false; + return turns == circle.turns; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + temp = Double.doubleToLongBits(radius); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + result = 31 * result + turns; + return result; + } + @Override public void writeToParcel(Parcel dest, int flags) { super.writeToParcel(dest, flags); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/DoLandStart.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/DoLandStart.java index 49169c6f24..72627318ef 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/DoLandStart.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/DoLandStart.java @@ -19,6 +19,11 @@ private DoLandStart(Parcel in) { super(in); } + @Override + public String toString() { + return "DoLandStart{ " + super.toString() + " }"; + } + @Override public MissionItem clone() { return new DoLandStart(this); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Land.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Land.java index a7b022814d..1e3777093d 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Land.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Land.java @@ -23,6 +23,11 @@ private Land(Parcel in) { super(in); } + @Override + public String toString() { + return "Land{ " + super.toString() + " }"; + } + @Override public MissionItem clone() { return new Land(this); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/RegionOfInterest.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/RegionOfInterest.java index a35342f64e..a6c0ee486f 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/RegionOfInterest.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/RegionOfInterest.java @@ -24,6 +24,11 @@ private RegionOfInterest(Parcel in) { super(in); } + @Override + public String toString() { + return "RegionOfInterest{ " + super.toString() + " }"; + } + @Override public MissionItem clone() { return new RegionOfInterest(this); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/SplineWaypoint.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/SplineWaypoint.java index 9f3c57789d..408e36208b 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/SplineWaypoint.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/SplineWaypoint.java @@ -45,6 +45,35 @@ private SplineWaypoint(Parcel in) { this.delay = in.readDouble(); } + @Override + public String toString() { + return "SplineWaypoint{" + + "delay=" + delay + + ", " + super.toString() + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof SplineWaypoint)) return false; + if (!super.equals(o)) return false; + + SplineWaypoint that = (SplineWaypoint) o; + + return Double.compare(that.delay, delay) == 0; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + temp = Double.doubleToLongBits(delay); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + return result; + } + @Override public MissionItem clone() { return new SplineWaypoint(this); diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Waypoint.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Waypoint.java index 068d6aa70d..f1df71ae71 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Waypoint.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Waypoint.java @@ -89,6 +89,50 @@ private Waypoint(Parcel in) { this.orbitCCW = in.readByte() != 0; } + @Override + public String toString() { + return "Waypoint{" + + "acceptanceRadius=" + acceptanceRadius + + ", delay=" + delay + + ", yawAngle=" + yawAngle + + ", orbitalRadius=" + orbitalRadius + + ", orbitCCW=" + orbitCCW + + ", " + super.toString() + + '}'; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (!(o instanceof Waypoint)) return false; + if (!super.equals(o)) return false; + + Waypoint waypoint = (Waypoint) o; + + if (Double.compare(waypoint.delay, delay) != 0) return false; + if (Double.compare(waypoint.acceptanceRadius, acceptanceRadius) != 0) return false; + if (Double.compare(waypoint.yawAngle, yawAngle) != 0) return false; + if (Double.compare(waypoint.orbitalRadius, orbitalRadius) != 0) return false; + return orbitCCW == waypoint.orbitCCW; + + } + + @Override + public int hashCode() { + int result = super.hashCode(); + long temp; + temp = Double.doubleToLongBits(delay); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(acceptanceRadius); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(yawAngle); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(orbitalRadius); + result = 31 * result + (int) (temp ^ (temp >>> 32)); + result = 31 * result + (orbitCCW ? 1 : 0); + return result; + } + @Override public MissionItem clone() { return new Waypoint(this); diff --git a/ClientLib/src/main/res/values/version.xml b/ClientLib/src/main/res/values/version.xml index b0f6ed4844..ce90820975 100644 --- a/ClientLib/src/main/res/values/version.xml +++ b/ClientLib/src/main/res/values/version.xml @@ -1,4 +1,4 @@ - 20809 + 20810 \ No newline at end of file From bbf96ed5b3be2ffeb5b717be413311bf6a8567a7 Mon Sep 17 00:00:00 2001 From: fredia Date: Mon, 29 Feb 2016 16:58:42 -0800 Subject: [PATCH 30/37] Override toString() for the `MissionItem` class --- .../android/lib/drone/mission/item/MissionItem.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/MissionItem.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/MissionItem.java index 3960efdcd1..f4c2fb8b5d 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/MissionItem.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/MissionItem.java @@ -67,4 +67,11 @@ public boolean equals(Object o) { public int hashCode() { return type != null ? type.hashCode() : 0; } + + @Override + public String toString() { + return "MissionItem{" + + "type=" + type + + '}'; + } } From 0ff559789a54c52678dacfc30f618deb44178e58 Mon Sep 17 00:00:00 2001 From: fredia Date: Mon, 29 Feb 2016 17:00:38 -0800 Subject: [PATCH 31/37] Override toString() for the `Circle` class --- .../android/lib/drone/mission/item/spatial/Circle.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Circle.java b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Circle.java index 9da7d5c04c..79c339153d 100644 --- a/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Circle.java +++ b/ClientLib/src/main/java/com/o3dr/services/android/lib/drone/mission/item/spatial/Circle.java @@ -40,6 +40,15 @@ public void setTurns(int turns) { this.turns = turns; } + @Override + public String toString() { + return "Circle{" + + "radius=" + radius + + ", turns=" + turns + + ", " + super.toString() + + '}'; + } + @Override public boolean equals(Object o) { if (this == o) return true; From fdb94f9eb2fc5ff2e2508216f1c1da954e1d952e Mon Sep 17 00:00:00 2001 From: fredia Date: Mon, 29 Feb 2016 17:20:14 -0800 Subject: [PATCH 32/37] Allow negative radius for a circle mission item --- .../services/android/core/mission/waypoints/CircleImpl.java | 2 +- .../android/core/mission/waypoints/StructureScannerImpl.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ServiceApp/src/org/droidplanner/services/android/core/mission/waypoints/CircleImpl.java b/ServiceApp/src/org/droidplanner/services/android/core/mission/waypoints/CircleImpl.java index 37f43372d6..3588ddafd0 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/mission/waypoints/CircleImpl.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/mission/waypoints/CircleImpl.java @@ -35,7 +35,7 @@ public void setTurns(int turns) { } public void setRadius(double radius) { - this.radius = Math.abs(radius); + this.radius = radius; } public int getNumberOfTurns() { diff --git a/ServiceApp/src/org/droidplanner/services/android/core/mission/waypoints/StructureScannerImpl.java b/ServiceApp/src/org/droidplanner/services/android/core/mission/waypoints/StructureScannerImpl.java index a502f32a27..219e5984a8 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/mission/waypoints/StructureScannerImpl.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/mission/waypoints/StructureScannerImpl.java @@ -132,7 +132,7 @@ public LatLong getCenter() { } public void setRadius(int newValue) { - radius = (newValue); + radius = newValue; } public void enableCrossHatch(boolean isEnabled) { From e7c09942aca5c62fa3a03f77b8f8823c97fc1963 Mon Sep 17 00:00:00 2001 From: fredia Date: Mon, 29 Feb 2016 19:04:32 -0800 Subject: [PATCH 33/37] Bump build for beta 3 --- ServiceApp/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ServiceApp/build.gradle b/ServiceApp/build.gradle index ec6d6f5970..6462fdce70 100644 --- a/ServiceApp/build.gradle +++ b/ServiceApp/build.gradle @@ -40,7 +40,7 @@ dependencies { def versionMajor = 1 def versionMinor = 5 def versionPatch = 2 -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 From dd14c1d157ee889239b513854ee826525c95ab72 Mon Sep 17 00:00:00 2001 From: Chavi Weingarten Date: Mon, 7 Mar 2016 15:40:49 -0800 Subject: [PATCH 34/37] Updated ClientLib version to release --- ClientLib/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index f91d31a55d..e7adf85cc5 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -4,7 +4,7 @@ ext { VERSION_MAJOR = 2 VERSION_MINOR = 8 VERSION_PATCH = 1 - VERSION_SUFFIX = "beta1" + VERSION_SUFFIX = "release" PUBLISH_ARTIFACT_ID = 'dronekit-android' PUBLISH_VERSION = generateVersionName("", VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH, VERSION_SUFFIX) From d3c0e3bcb946117d99de16f1fefc95d4dcf0c20b Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Mon, 7 Mar 2016 18:16:55 -0800 Subject: [PATCH 35/37] Version build bump for production release --- ServiceApp/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ServiceApp/build.gradle b/ServiceApp/build.gradle index 6462fdce70..5a2fa419af 100644 --- a/ServiceApp/build.gradle +++ b/ServiceApp/build.gradle @@ -40,7 +40,7 @@ dependencies { def versionMajor = 1 def versionMinor = 5 def versionPatch = 2 -def versionBuild = 3 //bump for dogfood builds, public betas, etc. +def versionBuild = 4 //bump for dogfood builds, public betas, etc. def versionPrefix = "3DR Services v" //Log levels values From a5b7da715f475e77b4ab670817bb3821b58a7daf Mon Sep 17 00:00:00 2001 From: ne0fhyk Date: Tue, 8 Mar 2016 00:12:57 -0800 Subject: [PATCH 36/37] Update client library dependencies to fix travis build issues --- ClientLib/build.gradle | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ClientLib/build.gradle b/ClientLib/build.gradle index e7adf85cc5..dd991d43e3 100644 --- a/ClientLib/build.gradle +++ b/ClientLib/build.gradle @@ -58,8 +58,8 @@ dependencies { compile "com.android.support:support-v4:${support_lib_version}" compile "com.google.android.gms:play-services-base:${play_services_version}" - debugCompile project(':Mavlink') - releaseCompile files('libs/Mavlink.jar') + compile project(':Mavlink') + compile files('libs/Mavlink.jar') } assemble.dependsOn ':Mavlink:jar' From 935b49ce53580071140de08e18d16f1304260a41 Mon Sep 17 00:00:00 2001 From: KiBa1215 <446592760@qq.com> Date: Tue, 16 Feb 2016 16:30:55 +0800 Subject: [PATCH 37/37] Receiving PX4 parameters supported --- .../assets/Parameters/ParameterMetaData.xml | 4124 +++++++++++++++++ .../core/drone/autopilot/px4/Px4Native.java | 39 + .../core/drone/profiles/ParameterManager.java | 53 +- .../drone/profiles/ParameterMetadataPX4.java | 75 + .../android/core/firmware/FirmwareType.java | 2 +- .../file/IO/ParameterMetadataLoaderPX4.java | 205 + 6 files changed, 4485 insertions(+), 13 deletions(-) create mode 100644 ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterMetadataPX4.java create mode 100644 ServiceApp/src/org/droidplanner/services/android/utils/file/IO/ParameterMetadataLoaderPX4.java diff --git a/ServiceApp/assets/Parameters/ParameterMetaData.xml b/ServiceApp/assets/Parameters/ParameterMetaData.xml index d933aa5806..805312d8ba 100644 --- a/ServiceApp/assets/Parameters/ParameterMetaData.xml +++ b/ServiceApp/assets/Parameters/ParameterMetaData.xml @@ -7457,4 +7457,4128 @@ ../libraries/APM_Control/AP_SteerController.cpp + + 3 + + + Speed controller bandwidth + Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness. + Hertz + 10 + 250 + + + Reverse direction + Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction. + 0 + 1 + + + Speed (RPM) controller gain + Speed (RPM) controller gain. Determines controller + aggressiveness; units are amp-seconds per radian. Systems with + higher rotational inertia (large props) will need gain increased; + systems with low rotational inertia (small props) may need gain + decreased. Higher values result in faster response, but may result + in oscillation and excessive overshoot. Lower values result in a + slower, smoother response. + amp-seconds per radian + 3 + 0.00 + 1.00 + + + Idle speed (e Hz) + Idle speed (e Hz) + Hertz + 3 + 0.0 + 100.0 + + + Spin-up rate (e Hz/s) + Spin-up rate (e Hz/s) + Hz/s + 5 + 1000 + + + Index of this ESC in throttle command messages. + Index of this ESC in throttle command messages. + Index + 0 + 15 + + + Extended status ID + Extended status ID + 1 + 1000000 + + + Extended status interval (µs) + Extended status interval (µs) + µs + 0 + 1000000 + + + ESC status interval (µs) + ESC status interval (µs) + µs + 1000000 + + + Motor current limit in amps + Motor current limit in amps. This determines the maximum + current controller setpoint, as well as the maximum allowable + current setpoint slew rate. This value should generally be set to + the continuous current rating listed in the motor’s specification + sheet, or set equal to the motor’s specified continuous power + divided by the motor voltage limit. + Amps + 3 + 1 + 80 + + + Motor Kv in RPM per volt + Motor Kv in RPM per volt. This can be taken from the motor’s + specification sheet; accuracy will help control performance but + some deviation from the specified value is acceptable. + RPM/v + 0 + 4000 + + + READ ONLY: Motor inductance in henries. + READ ONLY: Motor inductance in henries. This is measured on start-up. + henries + 3 + + + Number of motor poles. + Number of motor poles. Used to convert mechanical speeds to + electrical speeds. This number should be taken from the motor’s + specification sheet. + Poles + 2 + 40 + + + READ ONLY: Motor resistance in ohms + READ ONLY: Motor resistance in ohms. This is measured on start-up. When + tuning a new motor, check that this value is approximately equal + to the value shown in the motor’s specification sheet. + Ohms + 3 + + + Acceleration limit (V) + Acceleration limit (V) + Volts + 3 + 0.01 + 1.00 + + + Motor voltage limit in volts + Motor voltage limit in volts. The current controller’s + commanded voltage will never exceed this value. Note that this may + safely be above the nominal voltage of the motor; to determine the + actual motor voltage limit, divide the motor’s rated power by the + motor current limit. + Volts + 3 + 0 + + + + + Body angular rate process noise + + + Body angular acceleration process noise + + + Acceleration process noise + + + Magnet field vector process noise + + + Gyro measurement noise + + + Accel measurement noise + + + Mag measurement noise + + + Moment of inertia matrix diagonal entry (1, 1) + kg*m^2 + + + Moment of inertia matrix diagonal entry (2, 2) + kg*m^2 + + + Moment of inertia matrix diagonal entry (3, 3) + kg*m^2 + + + Moment of inertia enabled in estimator + If set to != 0 the moment of inertia will be used in the estimator + 0 + 1 + + + + + Complimentary filter accelerometer weight + 0 + 1 + 2 + + + Complimentary filter magnetometer weight + 0 + 1 + 2 + + + Complimentary filter external heading weight + 0 + 1 + + + Complimentary filter gyroscope bias weight + 0 + 1 + 2 + + + Magnetic declination, in degrees + This parameter is not used in normal operation, as the declination is looked up based on the GPS coordinates of the vehicle. + degrees + 2 + + + Enable automatic GPS based declination compensation + 0 + 1 + + + External heading usage mode (from Motion capture/Vision) + Set to 1 to use heading estimate from vision. + Set to 2 to use heading from motion capture + 0 + 2 + + + Enable acceleration compensation based on GPS + velocity + 1 + 2 + + + Gyro bias limit + 0 + 2 + rad/s + 3 + + + Threshold (of RMS) to warn about high vibration levels + 0.01 + 10 + 2 + + + + + Scaling factor for battery voltage sensor on PX4IO + 1 + 100000 + + + Scaling factor for battery voltage sensor on FMU v2 + + + Scaling factor for battery current sensor + + + Empty cell voltage + Defines the voltage where a single cell of the battery is considered empty. + V + 2 + + + Full cell voltage + Defines the voltage where a single cell of the battery is considered full. + V + 2 + + + Voltage drop per cell on 100% load + This implicitely defines the internal resistance to maximum current ratio and assumes linearity. + 0.0 + 1.5 + V + 2 + + + Number of cells + Defines the number of cells the attached battery consists of. + 1 + 10 + S + + + Battery capacity + Defines the capacity of the attached battery. + mA + 0 + + + + + Camera trigger interval + This parameter sets the time between two consecutive trigger events + 4.0 + 10000.0 + milliseconds + + + Camera trigger polarity + This parameter sets the polarity of the trigger (0 = ACTIVE_LOW, 1 = ACTIVE_HIGH ) + 0 + 1 + + + Camera trigger activation time + This parameter sets the time the trigger needs to pulled high or low. + milliseconds + + + Camera trigger mode + 0 disables the trigger, 1 sets it to enabled on command, 2 always on + 0 + 2 + + + Camera trigger pin + Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6) + 1 + 123456 + + + + + Circuit breaker for power supply check + Setting this parameter to 894281 will disable the power valid checks in the commander. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 894281 + + + Circuit breaker for rate controller output + Setting this parameter to 140253 will disable the rate controller uORB publication. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 140253 + + + Circuit breaker for IO safety + Setting this parameter to 22027 will disable IO safety. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 22027 + + + Circuit breaker for airspeed sensor + Setting this parameter to 162128 will disable the check for an airspeed sensor. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 162128 + + + Circuit breaker for flight termination + Setting this parameter to 121212 will disable the flight termination action. --> The IO driver will not do flight termination if requested by the FMU WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 121212 + + + Circuit breaker for engine failure detection + Setting this parameter to 284953 will disable the engine failure detection. If the aircraft is in engine failure mode the enine failure flag will be set to healthy WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 284953 + + + Circuit breaker for GPS failure detection + Setting this parameter to 240024 will disable the GPS failure detection. If this check is enabled, then the sensor check will fail if the GPS module is missing. It will also check for excessive signal noise on the GPS receiver and warn the user if detected. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 240024 + + + Circuit breaker for disabling buzzer + Setting this parameter to 782097 will disable the buzzer audio notification. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 782097 + + + Circuit breaker for USB link check + Setting this parameter to 197848 will disable the USB connected checks in the commander. WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + 0 + 197848 + + + + + Datalink loss mode enabled + Set to 1 to enable actions triggered when the datalink is lost. + 0 + 1 + + + Datalink loss time threshold + After this amount of seconds without datalink the data link lost mode triggers + 0 + 30 + second + + + Datalink regain time threshold + After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss' flag is set back to false + 0 + 30 + second + + + Engine Failure Throttle Threshold + Engine failure triggers only above this throttle value + 0.0 + 1.0 + 1 + + + Engine Failure Current/Throttle Threshold + Engine failure triggers only below this current value + 0.0 + 30.0 + ampere + 2 + + + Engine Failure Time Threshold + Engine failure triggers only if the throttle threshold and the current to throttle threshold are violated for this time + 0.0 + 60.0 + second + 1 + + + RC loss time threshold + After this amount of seconds without RC connection the rc lost flag is set to true + 0 + 35 + second + 1 + + + Home set horizontal threshold + The home position will be set if the estimated positioning accuracy is below the threshold. + 2 + 15 + meter + 2 + + + Home set vertical threshold + The home position will be set if the estimated positioning accuracy is below the threshold. + 5 + 25 + meter + 2 + + + Autosaving of params + If not equal to zero the commander will automatically save parameters to persistent storage once changed. Default is on, as the interoperability with currently deployed GCS solutions depends on parameters being sticky. Developers can default it to off. + 0 + 1 + + + RC control input mode + The default value of 0 requires a valid RC transmitter setup. Setting this to 1 disables RC input handling and the associated checks. A value of 2 will generate RC control data from manual input received via MAVLink instead of directly forwarding the manual input data. + 0 + 2 + + + Time-out for auto disarm after landing + A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be automatically disarmed in case a landing situation has been detected during this period. A value of zero means that automatic disarming is disabled. + 0 + + + + + Airfield home Lat + Latitude of airfield home waypoint + -900000000 + 900000000 + degrees * 1e7 + + + Airfield home Lon + Longitude of airfield home waypoint + -1800000000 + 1800000000 + degrees * 1e7 + + + Airfield home alt + Altitude of airfield home waypoint + -50 + m + + + Comms hold wait time + The amount of time in seconds the system should wait at the comms hold waypoint + 0.0 + seconds + + + Comms hold Lat + Latitude of comms hold waypoint + -900000000 + 900000000 + degrees * 1e7 + + + Comms hold Lon + Longitude of comms hold waypoint + -1800000000 + 1800000000 + degrees * 1e7 + + + Comms hold alt + Altitude of comms hold waypoint + -50 + 30000 + m + + + Airfield hole wait time + The amount of time in seconds the system should wait at the airfield home waypoint + 0.0 + seconds + + + Number of allowed Datalink timeouts + After more than this number of data link timeouts the aircraft returns home directly + 0 + 1000 + + + Skip comms hold wp + If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to airfield home + 0 + 1 + + + + + Attitude Roll Time Constant + This defines the latency between a roll step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. + 0.4 + 1.0 + seconds + + + Attitude Pitch Time Constant + This defines the latency between a pitch step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. + 0.2 + 1.0 + seconds + + + Pitch rate proportional gain + This defines how much the elevator input will be commanded depending on the current body angular rate error. + 0.005 + 1.0 + + + Pitch rate integrator gain + This gain defines how much control response will result out of a steady state error. It trims any constant error. + 0.005 + 0.5 + + + Maximum positive / up pitch rate + This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Maximum negative / down pitch rate + This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Pitch rate integrator limit + The portion of the integrator part in the control surface deflection is limited to this value + 0.0 + 1.0 + + + Roll rate proportional Gain + This defines how much the aileron input will be commanded depending on the current body angular rate error. + 0.005 + 1.0 + + + Roll rate integrator Gain + This gain defines how much control response will result out of a steady state error. It trims any constant error. + 0.005 + 0.2 + + + Roll Integrator Anti-Windup + The portion of the integrator part in the control surface deflection is limited to this value. + 0.0 + 1.0 + + + Maximum Roll Rate + This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Yaw rate proportional gain + This defines how much the rudder input will be commanded depending on the current body angular rate error. + 0.005 + 1.0 + + + Yaw rate integrator gain + This gain defines how much control response will result out of a steady state error. It trims any constant error. + 0.0 + 50.0 + + + Yaw rate integrator limit + The portion of the integrator part in the control surface deflection is limited to this value + 0.0 + 1.0 + + + Maximum Yaw Rate + This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Wheel steering rate proportional gain + This defines how much the wheel steering input will be commanded depending on the current body angular rate error. + 0.005 + 1.0 + + + Wheel steering rate integrator gain + This gain defines how much control response will result out of a steady state error. It trims any constant error. + 0.0 + 50.0 + + + Wheel steering rate integrator limit + The portion of the integrator part in the control surface deflection is limited to this value + 0.0 + 1.0 + + + Maximum wheel steering rate + This limits the maximum wheel steering rate the controller will output (in degrees per second). Setting a value of zero disables the limit. + 0.0 + 90.0 + deg/s + + + Roll rate feed forward + Direct feed forward from rate setpoint to control surface output. Use this to obtain a tigher response of the controller without introducing noise amplification. + 0.0 + 10.0 + + + Pitch rate feed forward + Direct feed forward from rate setpoint to control surface output + 0.0 + 10.0 + + + Yaw rate feed forward + Direct feed forward from rate setpoint to control surface output + 0.0 + 10.0 + + + Wheel steering rate feed forward + Direct feed forward from rate setpoint to control surface output + 0.0 + 10.0 + + + Minimal speed for yaw coordination + For airspeeds above this value, the yaw rate is calculated for a coordinated turn. Set to a very high value to disable. + m/s + + + Method used for yaw coordination + The param value sets the method used to calculate the yaw rate 0: open-loop zero lateral acceleration based on kinematic constraints 1: closed-loop: try to reduce lateral acceleration to 0 by measuring the acceleration + 0 + 1 + m/s + + + Minimum Airspeed + If the airspeed falls below this value, the TECS controller will try to increase airspeed more aggressively. + 0.0 + 40 + m/s + + + Trim Airspeed + The TECS controller tries to fly at this airspeed. + 0.0 + 40 + m/s + + + Maximum Airspeed + If the airspeed is above this value, the TECS controller will try to decrease airspeed more aggressively. + 0.0 + 40 + m/s + + + Roll Setpoint Offset + An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe. + -90.0 + 90.0 + deg + + + Pitch Setpoint Offset + An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe. + -90.0 + 90.0 + deg + + + Max Manual Roll + Max roll for manual control in attitude stabilized mode + 0.0 + 90.0 + deg + + + Max Manual Pitch + Max pitch for manual control in attitude stabilized mode + 0.0 + 90.0 + deg + + + Scale factor for flaps + 0.0 + 1.0 + + + Scale factor for flaperons + 0.0 + 1.0 + + + + + Minimum descent rate + This is the sink rate of the aircraft with the throttle set to THR_MIN and flown at the same airspeed as used to measure FW_T_CLMB_MAX. + + + Maximum descent rate + This sets the maximum descent rate that the controller will use. If this value is too large, the aircraft can over-speed on descent. This should be set to a value that can be achieved without exceeding the lower pitch angle limit and without over-speeding the aircraft. + + + TECS time constant + This is the time constant of the TECS control algorithm (in seconds). Smaller values make it faster to respond, larger values make it slower to respond. + + + TECS Throttle time constant + This is the time constant of the TECS throttle control algorithm (in seconds). Smaller values make it faster to respond, larger values make it slower to respond. + + + Throttle damping factor + This is the damping gain for the throttle demand loop. Increase to add damping to correct for oscillations in speed and height. + + + Integrator gain + This is the integrator gain on the control loop. Increasing this gain increases the speed at which speed and height offsets are trimmed out, but reduces damping and increases overshoot. + + + Maximum vertical acceleration + This is the maximum vertical acceleration (in metres/second square) either up or down that the controller will use to correct speed or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) allows for reasonably aggressive pitch changes if required to recover from under-speed conditions. + + + Complementary filter "omega" parameter for height + This is the cross-over frequency (in radians/second) of the complementary filter used to fuse vertical acceleration and barometric height to obtain an estimate of height rate and height. Increasing this frequency weights the solution more towards use of the barometer, whilst reducing it weights the solution more towards use of the accelerometer data. + + + Complementary filter "omega" parameter for speed + This is the cross-over frequency (in radians/second) of the complementary filter used to fuse longitudinal acceleration and airspeed to obtain an improved airspeed estimate. Increasing this frequency weights the solution more towards use of the arispeed sensor, whilst reducing it weights the solution more towards use of the accelerometer data. + + + Roll -> Throttle feedforward + Increasing this gain turn increases the amount of throttle that will be used to compensate for the additional drag created by turning. Ideally this should be set to approximately 10 x the extra sink rate in m/s created by a 45 degree bank turn. Increase this gain if the aircraft initially loses energy in turns and reduce if the aircraft initially gains energy in turns. Efficient high aspect-ratio aircraft (eg powered sailplanes) can use a lower value, whereas inefficient low aspect-ratio models (eg delta wings) can use a higher value. + + + Speed <--> Altitude priority + This parameter adjusts the amount of weighting that the pitch control applies to speed vs height errors. Setting it to 0.0 will cause the pitch control to control height and ignore speed errors. This will normally improve height accuracy but give larger airspeed errors. Setting it to 2.0 will cause the pitch control loop to control speed and ignore height errors. This will normally reduce airspeed errors, but give larger height errors. The default value of 1.0 allows the pitch control to simultaneously control height and speed. Note to Glider Pilots - set this parameter to 2.0 (The glider will adjust its pitch angle to maintain airspeed, ignoring changes in height). + + + Pitch damping factor + This is the damping gain for the pitch demand loop. Increase to add damping to correct for oscillations in height. The default value of 0.0 will work well provided the pitch to servo controller has been tuned properly. + + + Height rate P factor + + + Height rate FF factor + + + Speed rate P factor + + + + + Loiter time + The amount of time in seconds the system should do open loop loiter and wait for gps recovery before it goes into flight termination. + 0.0 + seconds + + + Open loop loiter roll + Roll in degrees during the open loop loiter + 0.0 + 30.0 + deg + + + Open loop loiter pitch + Pitch in degrees during the open loop loiter + -30.0 + 30.0 + deg + + + Open loop loiter thrust + Thrust value which is set during the open loop loiter + 0.0 + 1.0 + + + + + Geofence violation action + 0 = none, 1 = warning (default), 2 = loiter, 3 = return to launch, 4 = fight termination + 0 + 4 + + + Geofence altitude mode + Select which altitude reference should be used 0 = WGS84, 1 = AMSL + 0 + 1 + + + Geofence source + Select which position source should be used. Selecting GPS instead of global position makes sure that there is no dependence on the position estimator 0 = global position, 1 = GPS + 0 + 1 + + + Geofence counter limit + Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered + -1 + 10 + + + Max horizontal distance in meters + Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value. + -1 + meters + + + Max vertical distance in meters + Set to > 0 to activate a geofence action if vertical distance to home exceeds this value. + -1 + meters + + + + + Consider mount operation mode + If set to 1, mount mode will be enforced. + 0 + 1 + + + Auxilary switch to set mount operation mode + Set to 0 to disable manual mode control. If set to an auxilary switch: Switch off means the gimbal is put into safe/locked position. Switch on means the gimbal can move freely, and landing gear will be retracted if applicable. + 0 + 3 + + + + + L1 period + This is the L1 distance and defines the tracking point ahead of the aircraft its following. A value of 25 meters works for most aircraft. Shorten slowly during tuning until response is sharp without oscillation. + 1.0 + 100.0 + + + L1 damping + Damping factor for L1 control. + 0.6 + 0.9 + + + Cruise throttle + This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7. + 0.0 + 1.0 + + + Throttle max slew rate + Maximum slew rate for the commanded throttle + 0.0 + 1.0 + + + Negative pitch limit + The minimum negative pitch the controller will output. + -60.0 + 0.0 + degrees + + + Positive pitch limit + The maximum positive pitch the controller will output. + 0.0 + 60.0 + degrees + + + Controller roll limit + The maximum roll the controller will output. + 35.0 + 65.0 + degrees + + + Throttle limit max + This is the maximum throttle % that can be used by the controller. For overpowered aircraft, this should be reduced to a value that provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. + + + Throttle limit min + This is the minimum throttle % that can be used by the controller. For electric aircraft this will normally be set to zero, but can be set to a small non-zero value if a folding prop is fitted to prevent the prop from folding and unfolding repeatedly in-flight or to provide some aerodynamic drag from a turning prop to improve the descent rate. For aircraft with internal combustion engine this parameter should be set for desired idle rpm. + 0.0 + 1.0 + + + Throttle limit value before flare + This throttle value will be set as throttle limit at FW_LND_TLALT, before arcraft will flare. + 0.0 + 1.0 + + + Climbout Altitude difference + If the altitude error exceeds this parameter, the system will climb out with maximum throttle and minimum airspeed until it is closer than this distance to the desired altitude. Mostly used for takeoff waypoints / modes. Set to zero to disable climbout mode (not recommended). + 0.0 + 150.0 + + + Maximum climb rate + This is the best climb rate that the aircraft can achieve with the throttle set to THR_MAX and the airspeed set to the default value. For electric aircraft make sure this number can be achieved towards the end of flight when the battery voltage has reduced. The setting of this parameter can be checked by commanding a positive altitude change of 100m in loiter, RTL or guided mode. If the throttle required to climb is close to THR_MAX and the aircraft is maintaining airspeed, then this parameter is set correctly. If the airspeed starts to reduce, then the parameter is set to high, and if the throttle demand required to climb and maintain speed is noticeably less than FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or FW_THR_MAX reduced. + 2.0 + 10.0 + + + Landing slope angle + + + FW_LND_HVIRT + + + Landing flare altitude (relative to landing altitude) + meter + + + Landing throttle limit altitude (relative landing altitude) + Default of -1.0f lets the system default to applying throttle limiting at 2/3 of the flare altitude. + meter + + + Landing heading hold horizontal distance + + + Enable or disable usage of terrain estimate during landing + 0: disabled, 1: enabled + + + Takeoff and landing airspeed scale factor + Multiplying this factor with the minimum airspeed of the plane gives the target airspeed for takeoff and landing approach. + 1.0 + 1.5 + + + + + Multicopter max climb rate + Maximum vertical velocity allowed in the landed state (m/s up and down) + m/s + + + Multicopter max horizontal velocity + Maximum horizontal velocity allowed in the landed state (m/s) + m/s + + + Multicopter max rotation + Maximum allowed around each axis allowed in the landed state (degrees per second) + deg/s + + + Multicopter max throttle + Maximum actuator output on throttle allowed in the landed state + 0.1 + 0.5 + + + Fixedwing max horizontal velocity + Maximum horizontal velocity allowed in the landed state (m/s) + 0.5 + 10 + m/s + + + Fixedwing max climb rate + Maximum vertical velocity allowed in the landed state (m/s up and down) + 5 + 20 + m/s + + + Fixedwing max short-term velocity + Maximum velocity integral in flight direction allowed in the landed state (m/s) + 2 + 10 + m/s + + + Airspeed max + Maximum airspeed allowed in the landed state (m/s) + 4 + 20 + m/s + + + + + Enable launch detection + 0 + 1 + + + Catapult accelerometer theshold + LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + 0 + + + Catapult time theshold + LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + 0 + + + Motor delay + Delay between starting attitude control and powering up the throttle (giving throttle control to the controller) Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate + 0 + seconds + + + Maximum pitch before the throttle is powered up (during motor delay phase) + This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on. This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep). + 0 + 45 + deg + + + Throttle setting while detecting launch + The throttle is set to this value while the system is waiting for the take-off. + 0 + 1 + + + + + Enable local position estimator + + + Enable accelerometer integration for prediction + + + Optical flow xy standard deviation + 0.01 + 1 + m + + + Sonar z standard deviation + 0.01 + 1 + m + + + Lidar z standard deviation + 0.01 + 1 + m + + + Accelerometer xy standard deviation + Data sheet sqrt(Noise power) = 150ug/sqrt(Hz) std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2 Since accels sampled at 1000 Hz. should be 0.0464 + 0.00001 + 2 + m/s^2 + + + Accelerometer z standard deviation + (see Accel x comments) + 0.00001 + 2 + m/s^2 + + + Barometric presssure altitude z standard deviation + 0.01 + 3 + m + + + GPS xy standard deviation + 0.01 + 5 + m + + + GPS z standard deviation + 0.01 + 20 + m + + + GPS xy velocity standard deviation + 0.01 + 2 + m/s + + + GPS z velocity standard deviation + 0.01 + 2 + m/s + + + GPS max eph + 1.0 + 5.0 + m + + + Vision xy standard deviation + 0.01 + 1 + m + + + Vision z standard deviation + 0.01 + 2 + m + + + Circuit breaker to disable vision input + Set to the appropriate key (328754) to disable vision input. + 0 + 1 + + + Vicon position standard deviation + 0.01 + 1 + m + + + Position propagation process noise power (variance*sampling rate) + 0 + 1 + (m/s^2)-s + + + Velocity propagation process noise power (variance*sampling rate) + 0 + 5 + (m/s)-s + + + Accel bias propagation process noise power (variance*sampling rate) + 0 + 1 + (m/s)-s + + + Fault detection threshold, for chi-squared dist + TODO add separate params for 1 dof, 3 dof, and 6 dof beta or false alarm rate in false alarms/hr + 3 + 1000 + + + + + + MAVLink system ID + 1 + 250 + + + MAVLink component ID + 1 + 250 + + + MAVLink Radio ID + When non-zero the MAVLink app will attempt to configure the radio to this ID and re-set the parameter to 0. If the value is negative it will reset the complete radio config to factory defaults. + -1 + 240 + + + MAVLink airframe type + 1 + + + Use/Accept HIL GPS message even if not in HIL mode + If set to 1 incoming HIL GPS messages are parsed. + + + Forward external setpoint messages + If set to 1 incoming external setpoint messages will be directly forwarded to the controllers if in offboard control mode + + + Test parameter + This parameter is not actively used by the system. Its purpose is to allow testing the parameter interface on the communication level. + -1000 + 1000 + + + + + Enables testmode (Identify) of MKBLCTRL Driver + + + + + Take-off altitude + Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to MIS_TAKEOFF_ALT on takeoff, then go to waypoint. + meters + + + Enable persistent onboard mission storage + When enabled, missions that have been uploaded by the GCS are stored and reloaded after reboot persistently. + 0 + 1 + + + Maximal horizontal distance from home to first waypoint + Failsafe check to prevent running mission stored from previous flight at a new takeoff location. Set a value of zero or less to disable. The mission will not be started if the current waypoint is more distant than MIS_DIS_1WP from the current position. + 0 + 1000 + + + Altitude setpoint mode + 0: the system will follow a zero order hold altitude setpoint 1: the system will follow a first order hold altitude setpoint values follow the definition in enum mission_altitude_mode + 0 + 1 + + + Multirotor only. Yaw setpoint mode + 0: Set the yaw heading to the yaw value specified for the destination waypoint. 1: Maintain a yaw heading pointing towards the next waypoint. 2: Maintain a yaw heading that always points to the home location. 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home). The values are defined in the enum mission_altitude_mode + 0 + 3 + + + Loiter radius (FW only) + Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). + 25 + 1000 + meter + + + Acceptance Radius + Default acceptance radius, overridden by acceptance radius of waypoint if set. + 0.05 + 200.0 + meter + + + Set OBC mode for data link loss + If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + 0 + 1 + + + Set OBC mode for rc loss + If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + 0 + 1 + + + + + Max manual roll + 0.0 + 90.0 + deg + + + Max manual pitch + 0.0 + 90.0 + deg + + + Max manual yaw rate + 0.0 + deg/s + + + Roll P gain + Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + + + Roll rate P gain + Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Roll rate I gain + Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Roll rate D gain + Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Pitch P gain + Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + 1/s + + + Pitch rate P gain + Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Pitch rate I gain + Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Pitch rate D gain + Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Yaw P gain + Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + 1/s + + + Yaw rate P gain + Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Yaw rate I gain + Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Yaw rate D gain + Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Yaw feed forward + Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Max yaw rate + Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + 0.0 + 360.0 + deg/s + + + Max acro roll rate + 0.0 + 360.0 + deg/s + + + Max acro pitch rate + 0.0 + 360.0 + deg/s + + + Max acro yaw rate + 0.0 + deg/s + + + Roll P gain + Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + + + Roll rate P gain + Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Roll rate I gain + Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Roll rate D gain + Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Roll rate feedforward + Improves tracking performance. + 0.0 + + + Pitch P gain + Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + 1/s + + + Pitch rate P gain + Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Pitch rate I gain + Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Pitch rate D gain + Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Pitch rate feedforward + Improves tracking performance. + 0.0 + + + Yaw P gain + Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + 0.0 + 1/s + + + Yaw rate P gain + Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + 0.0 + + + Yaw rate I gain + Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + 0.0 + + + Yaw rate D gain + Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + 0.0 + + + Yaw rate feedforward + Improves tracking performance. + 0.0 + + + Yaw feed forward + Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Max roll rate + Limit for roll rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + 0.0 + 360.0 + deg/s + + + Max pitch rate + Limit for pitch rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. + 0.0 + 360.0 + deg/s + + + Max yaw rate + Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation. A value of significantly over 60 degrees per second can already lead to mixer saturation. + 0.0 + 360.0 + deg/s + + + Max acro roll rate + 0.0 + 360.0 + deg/s + + + Max acro pitch rate + 0.0 + 360.0 + deg/s + + + Max acro yaw rate + 0.0 + deg/s + + + Threshold for Rattitude mode + Manual input needed in order to override attitude control rate setpoints and instead pass manual stick inputs as rate setpoints + 0.0 + 1.0 + + + + + + Minimum thrust + Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + 0.0 + 1.0 + + + Maximum thrust + Limit max allowed thrust. + 0.0 + 1.0 + + + Proportional gain for vertical position error + 0.0 + + + Proportional gain for vertical velocity error + 0.0 + + + Integral gain for vertical velocity error + Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + 0.0 + + + Differential gain for vertical velocity error + 0.0 + + + Maximum vertical velocity + Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL). + 0.0 + m/s + + + Vertical velocity feed forward + Feed forward weight for altitude control in stabilized modes (ALTCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Proportional gain for horizontal position error + 0.0 + + + Proportional gain for horizontal velocity error + 0.0 + + + Integral gain for horizontal velocity error + Non-zero value allows to resist wind. + 0.0 + + + Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again + 0.0 + + + Maximum horizontal velocity + Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). + 0.0 + m/s + + + Horizontal velocity feed forward + Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Maximum tilt angle in air + Limits maximum tilt in AUTO and POSCTRL modes during flight. + 0.0 + 90.0 + deg + + + Maximum tilt during landing + Limits maximum tilt angle on landing. + 0.0 + 90.0 + deg + + + Landing descend rate + 0.0 + m/s + + + Minimum thrust in auto thrust control + It's recommended to set it > 0 to avoid free fall with zero thrust. + 0.05 + 1.0 + + + Maximum thrust in auto thrust control + Limit max allowed thrust. Setting a value of one can put the system into actuator saturation as no spread between the motors is possible any more. A value of 0.8 - 0.9 is recommended. + 0.0 + 0.95 + + + Minimum manual thrust + Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + 0.0 + 1.0 + + + Maximum manual thrust + Limit max allowed thrust. Setting a value of one can put the system into actuator saturation as no spread between the motors is possible any more. A value of 0.8 - 0.9 is recommended. + 0.0 + 1.0 + + + Proportional gain for vertical position error + 0.0 + + + Proportional gain for vertical velocity error + 0.0 + + + Integral gain for vertical velocity error + Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + 0.0 + + + Differential gain for vertical velocity error + 0.0 + + + Maximum vertical velocity + Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). + 0.0 + 8.0 + m/s + + + Vertical velocity feed forward + Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Proportional gain for horizontal position error + 0.0 + + + Proportional gain for horizontal velocity error + 0.0 + + + Integral gain for horizontal velocity error + Non-zero value allows to resist wind. + 0.0 + + + Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again + 0.0 + + + Maximum horizontal velocity + Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). + 0.0 + m/s + + + Horizontal velocity feed forward + Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + 0.0 + 1.0 + + + Maximum tilt angle in air + Limits maximum tilt in AUTO and POSCTRL modes during flight. + 0.0 + 90.0 + degree + + + Maximum tilt during landing + Limits maximum tilt angle on landing. + 0.0 + 90.0 + degree + + + Landing descend rate + 0.0 + m/s + + + Max manual roll + 0.0 + 90.0 + degree + + + Max manual pitch + 0.0 + 90.0 + degree + + + Max manual yaw rate + 0.0 + degree / s + + + Deadzone of X,Y sticks where position hold is enabled + 0.0 + 1.0 + % + + + Deadzone of Z stick where altitude hold is enabled + 0.0 + 1.0 + % + + + Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) + 0.0 + m/s + + + Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + 0.0 + m/s + + + Low pass filter cut freq. for numerical velocity derivative + 0.0 + Hz + + + Maximum horizonal acceleration in velocity controlled modes + 2.0 + 10.0 + m/s/s + + + + + Set the minimum PWM for the MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. Set to 1000 for industry default or 900 to increase servo travel. + 800 + 1400 + microseconds + + + Set the maximum PWM for the MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. Set to 2000 for industry default or 2100 to increase servo travel. + 1600 + 2200 + microseconds + + + Set the disarmed PWM for MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. This is the PWM pulse the autopilot is outputting if not armed. The main use of this parameter is to silence ESCs when they are disarmed. + 0 + 2200 + microseconds + + + Set the minimum PWM for the MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. Set to 1000 for default or 900 to increase servo travel + 800 + 1400 + microseconds + + + Set the maximum PWM for the MAIN outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. Set to 2000 for default or 2100 to increase servo travel + 1600 + 2200 + microseconds + + + Set the disarmed PWM for AUX outputs + IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE THE SYSTEM TO PUT CHANGES INTO EFFECT. This is the PWM pulse the autopilot is outputting if not armed. The main use of this parameter is to silence ESCs when they are disarmed. + 0 + 2200 + microseconds + + + Invert direction of main output channel 1 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 2 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 3 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 4 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 5 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 6 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 7 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of main output channel 8 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Enable S.BUS out + Set to 1 to enable S.BUS version 1 output instead of RSSI. + 0 + 1 + + + Invert direction of aux output channel 1 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 2 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 3 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 4 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 5 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + Invert direction of aux output channel 6 + Set to 1 to invert the channel, 0 for default direction. + 0 + 1 + + + + + Ground drag property + This parameter encodes the ground drag coefficient and the corresponding decrease in wind speed from the plane altitude to ground altitude. + 0.001 + 0.1 + unknown + + + Plane turn radius + The planes known minimal turn radius - use a higher value to make the plane maneuver more distant from the actual drop position. This is to ensure the wings are level during the drop. + 30.0 + 500.0 + meter + + + Drop precision + If the system is closer than this distance on passing over the drop position, it will release the payload. This is a safeguard to prevent a drop out of the required accuracy. + 1.0 + 80.0 + meter + + + Payload drag coefficient of the dropped object + The drag coefficient (cd) is the typical drag constant for air. It is in general object specific, but the closest primitive shape to the actual object should give good results: http://en.wikipedia.org/wiki/Drag_coefficient + 0.08 + 1.5 + meter + + + Payload mass + A typical small toy ball: 0.025 kg OBC water bottle: 0.6 kg + 0.001 + 5.0 + kilogram + + + Payload front surface area + A typical small toy ball: (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2 OBC water bottle: (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2 + 0.001 + 0.5 + m^2 + + + + + Velocity estimate delay + The delay in milliseconds of the velocity estimate from GPS. + 0 + 1000 + + + Position estimate delay + The delay in milliseconds of the position estimate from GPS. + 0 + 1000 + + + Height estimate delay + The delay in milliseconds of the height estimate from the barometer. + 0 + 1000 + + + Mag estimate delay + The delay in milliseconds of the magnetic field estimate from the magnetometer. + 0 + 1000 + + + True airspeeed estimate delay + The delay in milliseconds of the airspeed estimate. + 0 + 1000 + + + GPS vs. barometric altitude update weight + RE-CHECK this. + 0.0 + 1.0 + + + Airspeed measurement noise + Increasing this value will make the filter trust this sensor less and trust other sensors more. + 0.5 + 5.0 + + + Velocity measurement noise in north-east (horizontal) direction + Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 + 0.05 + 5.0 + + + Velocity noise in down (vertical) direction + Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7 + 0.2 + 3.0 + + + Position noise in north-east (horizontal) direction + Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5 + 0.1 + 10.0 + + + Position noise in down (vertical) direction + Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0 + 0.5 + 3.0 + + + Magnetometer measurement noise + Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 + 0.01 + 1.0 + + + Gyro process noise + Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015. This noise controls how much the filter trusts the gyro measurements. Increasing it makes the filter trust the gyro less and other sensors more. + 0.001 + 0.05 + + + Accelerometer process noise + Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25. Increasing this value makes the filter trust the accelerometer less and other sensors more. + 0.05 + 1.0 + + + Gyro bias estimate process noise + Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. Increasing this value will make the gyro bias converge faster but noisier. + 0.00000005 + 0.00001 + + + Accelerometer bias estimate process noise + Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f. Increasing this value makes the bias estimation faster and noisier. + 0.00001 + 0.001 + + + Magnetometer earth frame offsets process noise + Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001. Increasing this value makes the magnetometer earth bias estimate converge faster but also noisier. + 0.0001 + 0.01 + + + Magnetometer body frame offsets process noise + Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003. Increasing this value makes the magnetometer body bias estimate converge faster but also noisier. + 0.0001 + 0.01 + + + Magnetometer X bias + The magnetometer bias. This bias is learnt by the filter over time and persists between boots. + -0.6 + 0.6 + + + Magnetometer Y bias + The magnetometer bias. This bias is learnt by the filter over time and persists between boots. + -0.6 + 0.6 + + + Magnetometer Z bias + The magnetometer bias. This bias is learnt by the filter over time and persists between boots. + -0.6 + 0.6 + + + Threshold for filter initialization + If the standard deviation of the GPS position estimate is below this threshold in meters, the filter will initialize. + 0.3 + 10.0 + + + + + Z axis weight for barometer + Weight (cutoff frequency) for barometer altitude measurements. + 0.0 + 10.0 + + + Z axis weight for GPS + Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. + 0.0 + 10.0 + + + Z velocity weight for GPS + Weight (cutoff frequency) for GPS altitude velocity measurements. + 0.0 + 10.0 + + + Z axis weight for vision + Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset. + 0.0 + 10.0 + + + Z axis weight for lidar + Weight (cutoff frequency) for lidar measurements. + 0.0 + 10.0 + + + XY axis weight for GPS position + Weight (cutoff frequency) for GPS position measurements. + 0.0 + 10.0 + + + XY axis weight for GPS velocity + Weight (cutoff frequency) for GPS velocity measurements. + 0.0 + 10.0 + + + XY axis weight for vision position + Weight (cutoff frequency) for vision position measurements. + 0.0 + 10.0 + + + XY axis weight for vision velocity + Weight (cutoff frequency) for vision velocity measurements. + 0.0 + 10.0 + + + Weight for mocap system + Weight (cutoff frequency) for mocap position measurements. + 0.0 + 10.0 + + + XY axis weight for optical flow + Weight (cutoff frequency) for optical flow (velocity) measurements. + 0.0 + 10.0 + + + XY axis weight for resetting velocity + When velocity sources lost slowly decrease estimated horizontal velocity with this weight. + 0.0 + 10.0 + + + XY axis weight factor for GPS when optical flow available + When optical flow data available, multiply GPS weights (for position and velocity) by this factor. + 0.0 + 1.0 + + + Accelerometer bias estimation weight + Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + 0.0 + 0.1 + + + Optical flow scale factor + Factor to convert raw optical flow (in pixels) to radians [rad/px]. + 0.0 + 1.0 + rad/px + + + Minimal acceptable optical flow quality + 0 - lowest quality, 1 - best quality. + 0.0 + 1.0 + + + Weight for lidar filter + Lidar filter detects spikes on lidar measurements and used to detect new surface level. + 0.0 + 1.0 + + + Sonar maximal error for new surface + If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). + 0.0 + 1.0 + m + + + Land detector time + Vehicle assumed landed if no altitude changes happened during this time on low throttle. + 0.0 + 10.0 + s + + + Land detector altitude dispersion threshold + Dispersion threshold for triggering land detector. + 0.0 + 10.0 + m + + + Land detector throttle threshold + Value should be lower than minimal hovering thrust. Half of it is good choice. + 0.0 + 1.0 + + + GPS delay + GPS delay compensation + 0.0 + 1.0 + s + + + Flow module offset (center of rotation) in X direction + Yaw X flow compensation + -1.0 + 1.0 + m + + + Flow module offset (center of rotation) in Y direction + Yaw Y flow compensation + -1.0 + 1.0 + m + + + Disable mocap (set 0 if using fake gps) + Disable mocap + 0 + 1 + + + Disable vision input + Set to the appropriate key (328754) to disable vision input. + 0 + 1 + + + INAV enabled + If set to 1, use INAV for position estimation. Else the system uses the combined attitude / position filter framework. + 0 + 1 + + + + + RC Channel 1 Minimum + Minimum value for RC channel 1 + 800.0 + 1500.0 + us + + + RC Channel 1 Trim + Mid point value (same as min for throttle) + 800.0 + 2200.0 + us + + + RC Channel 1 Maximum + Maximum value for RC channel 1 + 1500.0 + 2200.0 + us + + + RC Channel 1 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 1 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + us + + + RC Channel 2 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 2 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 2 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 2 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 2 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + us + + + RC Channel 3 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 3 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 3 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 3 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 3 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + us + + + RC Channel 4 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 4 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 4 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 4 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 4 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + us + + + RC Channel 5 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 5 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 5 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 5 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 5 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 6 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + + + RC Channel 6 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 6 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 6 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 6 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 7 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + + + RC Channel 7 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 7 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 7 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 7 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 8 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 8 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 8 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 8 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 8 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 9 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 9 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 9 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 9 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 9 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 10 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 10 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 10 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 10 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 10 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 11 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 11 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 11 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 11 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 11 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 12 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 12 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 12 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 12 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 12 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 13 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 13 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 13 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 13 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 13 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 14 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 14 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 14 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 14 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 14 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 15 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 15 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 15 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 15 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 15 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 16 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 16 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 16 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 16 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 16 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 17 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 17 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 17 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 17 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 17 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + RC Channel 18 Minimum + Minimum value for this channel. + 800.0 + 1500.0 + us + + + RC Channel 18 Trim + Mid point value (has to be set to the same as min for throttle channel). + 800.0 + 2200.0 + us + + + RC Channel 18 Maximum + Maximum value for this channel. + 1500.0 + 2200.0 + us + + + RC Channel 18 Reverse + Set to -1 to reverse channel. + -1.0 + 1.0 + + + RC Channel 18 dead zone + The +- range of this value around the trim value will be considered as zero. + 0.0 + 100.0 + + + Enable relay control of relay 1 mapped to the Spektrum receiver power supply + 0 + 1 + + + DSM binding trigger + -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind + + + RC channel count + This parameter is used by Ground Station software to save the number of channels which were used during RC calibration. It is only meant for ground station use. + 0 + 18 + + + RC mode switch threshold automaic distribution + This parameter is used by Ground Station software to specify whether the threshold values for flight mode switches were automatically calculated. 0 indicates that the threshold values were set by the user. Any other value indicates that the threshold value where automatically set by the ground station software. It is only meant for ground station use. + 0 + 1 + + + Roll control channel mapping + The channel index (starting from 1 for channel 1) indicates which channel should be used for reading roll inputs from. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Pitch control channel mapping + The channel index (starting from 1 for channel 1) indicates which channel should be used for reading pitch inputs from. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Throttle control channel mapping + The channel index (starting from 1 for channel 1) indicates which channel should be used for reading throttle inputs from. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Yaw control channel mapping + The channel index (starting from 1 for channel 1) indicates which channel should be used for reading yaw inputs from. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Auxiliary switch 1 channel mapping + Default function: Camera pitch + 0 + 18 + + + Auxiliary switch 2 channel mapping + Default function: Camera roll + 0 + 18 + + + Auxiliary switch 3 channel mapping + Default function: Camera azimuth / yaw + 0 + 18 + + + Channel which changes a parameter + Can be used for parameter tuning with the RC. This one is further referenced as the 1st parameter channel. Set to 0 to deactivate * + 0 + 18 + + + Channel which changes a parameter + Can be used for parameter tuning with the RC. This one is further referenced as the 2nd parameter channel. Set to 0 to deactivate * + 0 + 18 + + + Channel which changes a parameter + Can be used for parameter tuning with the RC. This one is further referenced as the 3th parameter channel. Set to 0 to deactivate * + 0 + 18 + + + Failsafe channel PWM threshold + Set to a value slightly above the PWM value assumed by throttle in a failsafe event, but ensure it is below the PWM value assumed by throttle during normal operation. + 0 + 2200 + us + + + PWM input channel that provides RSSI + 0: do not read RSSI from input channel 1-18: read RSSI from specified input channel Specify the range for RSSI input with RC_RSSI_PWM_MIN and RC_RSSI_PWM_MAX parameters. + 0 + 18 + + + Max input value for RSSI reading + Only used if RC_RSSI_PWM_CHAN > 0 + 0 + 2000 + + + Min input value for RSSI reading + Only used if RC_RSSI_PWM_CHAN > 0 + 0 + 2000 + + + Roll trim + The trim value is the actuator control value the system needs for straight and level flight. It can be calibrated by flying manually straight and level using the RC trims and copying them using the GCS. + -0.25 + 0.25 + 2 + + + Pitch trim + The trim value is the actuator control value the system needs for straight and level flight. It can be calibrated by flying manually straight and level using the RC trims and copying them using the GCS. + -0.25 + 0.25 + 2 + + + Yaw trim + The trim value is the actuator control value the system needs for straight and level flight. It can be calibrated by flying manually straight and level using the RC trims and copying them using the GCS. + -0.25 + 0.25 + 2 + + + + + Loiter Time + The amount of time in seconds the system should loiter at current position before termination Set to -1 to make the system skip loitering + -1.0 + seconds + + + + + Mode switch channel mapping + This is the main flight mode selector. The channel index (starting from 1 for channel 1) indicates which channel should be used for deciding about the main mode. A value of zero indicates the switch is not assigned. + 0 + 18 + + + Return switch channel mapping + 0 + 18 + + + Rattitude switch channel mapping + 0 + 18 + + + Posctl switch channel mapping + 0 + 18 + + + Loiter switch channel mapping + 0 + 18 + + + Acro switch channel mapping + 0 + 18 + + + Offboard switch channel mapping + 0 + 18 + + + Flaps channel mapping + 0 + 18 + + + Threshold for selecting assist mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting auto mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting rattitude mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting posctl mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting return to launch mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting loiter mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting acro mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + Threshold for selecting offboard mode + 0-1 indicate where in the full channel range the threshold sits 0 : min 1 : max sign indicates polarity of comparison positive : true when channel>th negative : true when channel<th + -1 + 1 + + + + + RTL altitude + Altitude to fly back in RTL in meters + 0 + 150 + meters + + + RTL loiter altitude + Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. + 2 + 100 + meters + + + RTL delay + Delay after descend before landing in RTL mode. If set to -1 the system will not land but loiter at NAV_LAND_ALT. + -1 + 300 + seconds + + + + + Enable or disable runway takeoff with landing gear + 0: disabled, 1: enabled + 0 + 1 + + + Specifies which heading should be held during runnway takeoff + 0: airframe heading, 1: heading towards takeoff waypoint + 0 + 1 + + + Altitude AGL at which we have enough ground clearance to allow some roll. + Until RWTO_NAV_ALT is reached the plane is held level and only + rudder is used to keep the heading (see RWTO_HDG). This should be below + FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0 + 0.0 + 100.0 + m + + + Max throttle during runway takeoff. + (Can be used to test taxi on runway) + 0.0 + 1.0 + + + Pitch setpoint during taxi / before takeoff airspeed is reached. + A taildragger with stearable wheel might need to pitch up + a little to keep it's wheel on the ground before airspeed + to takeoff is reached + 0.0 + 20.0 + deg + + + Max pitch during takeoff. + Fixed-wing settings are used if set to 0. Note that there is also a minimum + pitch of 10 degrees during takeoff, so this must be larger if set + 0.0 + 60.0 + deg + + + Max roll during climbout. + Roll is limited during climbout to ensure enough lift and prevents aggressive + navigation before we're on a safe height + 0.0 + 60.0 + deg + + + Min. airspeed scaling factor for takeoff. + Pitch up will be commanded when the following airspeed is reached: + FW_AIRSPD_MIN * RWTO_AIRSPD_SCL + 0.0 + 2.0 + + + + + Logging rate + A value of -1 indicates the commandline argument should be obeyed. A value of 0 sets the minimum rate, any other value is interpreted as rate in Hertz. This parameter is only read out before logging starts (which commonly is before arming). + -1 + 100 + + + Enable extended logging mode + A value of -1 indicates the commandline argument should be obeyed. A value of 0 disables extended logging mode, a value of 1 enables it. This parameter is only read out before logging starts (which commonly is before arming). + -1 + 1 + + + Use timestamps only if GPS 3D fix is available + A value of 1 constrains the log folder creation to only use the time stamp if a 3D GPS lock is present. + 0 + 1 + + + + + ID of the board this parameter set was calibrated on + + + ID of the Gyro that the calibration is for + + + Gyro X-axis offset + -10.0 + 10.0 + + + Gyro Y-axis offset + -10.0 + 10.0 + + + Gyro Z-axis offset + -5.0 + 5.0 + + + Gyro X-axis scaling factor + -1.5 + 1.5 + + + Gyro Y-axis scaling factor + -1.5 + 1.5 + + + Gyro Z-axis scaling factor + -1.5 + 1.5 + + + ID of Magnetometer the calibration is for + + + Rotation of magnetometer 0 relative to airframe + An internal magnetometer will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + -1 + 30 + + + Magnetometer X-axis offset + -500.0 + 500.0 + + + Magnetometer Y-axis offset + -500.0 + 500.0 + + + Magnetometer Z-axis offset + -500.0 + 500.0 + + + Magnetometer X-axis scaling factor + + + Magnetometer Y-axis scaling factor + + + Magnetometer Z-axis scaling factor + + + ID of the Accelerometer that the calibration is for + + + Accelerometer X-axis offset + + + Accelerometer Y-axis offset + + + Accelerometer Z-axis offset + + + Accelerometer X-axis scaling factor + + + Accelerometer Y-axis scaling factor + + + Accelerometer Z-axis scaling factor + + + ID of the Gyro that the calibration is for + + + Gyro X-axis offset + -10.0 + 10.0 + + + Gyro Y-axis offset + -10.0 + 10.0 + + + Gyro Z-axis offset + -5.0 + 5.0 + + + Gyro X-axis scaling factor + -1.5 + 1.5 + + + Gyro Y-axis scaling factor + -1.5 + 1.5 + + + Gyro Z-axis scaling factor + -1.5 + 1.5 + + + ID of Magnetometer the calibration is for + + + Rotation of magnetometer 1 relative to airframe + An internal magnetometer will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + -1 + 30 + + + Magnetometer X-axis offset + -500.0 + 500.0 + + + Magnetometer Y-axis offset + -500.0 + 500.0 + + + Magnetometer Z-axis offset + -500.0 + 500.0 + + + Magnetometer X-axis scaling factor + + + Magnetometer Y-axis scaling factor + + + Magnetometer Z-axis scaling factor + + + ID of the Accelerometer that the calibration is for + + + Accelerometer X-axis offset + + + Accelerometer Y-axis offset + + + Accelerometer Z-axis offset + + + Accelerometer X-axis scaling factor + + + Accelerometer Y-axis scaling factor + + + Accelerometer Z-axis scaling factor + + + ID of the Gyro that the calibration is for + + + Gyro X-axis offset + -10.0 + 10.0 + + + Gyro Y-axis offset + -10.0 + 10.0 + + + Gyro Z-axis offset + -5.0 + 5.0 + + + Gyro X-axis scaling factor + -1.5 + 1.5 + + + Gyro Y-axis scaling factor + -1.5 + 1.5 + + + Gyro Z-axis scaling factor + -1.5 + 1.5 + + + ID of Magnetometer the calibration is for + + + Rotation of magnetometer 2 relative to airframe + An internal magnetometer will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + -1 + 30 + + + Magnetometer X-axis offset + -500.0 + 500.0 + + + Magnetometer Y-axis offset + -500.0 + 500.0 + + + Magnetometer Z-axis offset + -500.0 + 500.0 + + + Magnetometer X-axis scaling factor + + + Magnetometer Y-axis scaling factor + + + Magnetometer Z-axis scaling factor + + + ID of the Accelerometer that the calibration is for + + + Accelerometer X-axis offset + + + Accelerometer Y-axis offset + + + Accelerometer Z-axis offset + + + Accelerometer X-axis scaling factor + + + Accelerometer Y-axis scaling factor + + + Accelerometer Z-axis scaling factor + + + Primary accel ID + + + Primary gyro ID + + + Primary mag ID + + + Primary baro ID + + + Differential pressure sensor offset + The offset (zero-reading) in Pascal + + + Differential pressure sensor analog scaling + Pick the appropriate scaling from the datasheet. this number defines the (linear) conversion from voltage to Pascal (pa). For the MPXV7002DP this is 1000. NOTE: If the sensor always registers zero, try switching the static and dynamic tubes. + + + QNH for barometer + 500 + 1500 + hPa + + + Board rotation + This parameter defines the rotation of the FMU board relative to the platform. Possible values are: 0 = No rotation 1 = Yaw 45° 2 = Yaw 90° 3 = Yaw 135° 4 = Yaw 180° 5 = Yaw 225° 6 = Yaw 270° 7 = Yaw 315° 8 = Roll 180° 9 = Roll 180°, Yaw 45° 10 = Roll 180°, Yaw 90° 11 = Roll 180°, Yaw 135° 12 = Pitch 180° 13 = Roll 180°, Yaw 225° 14 = Roll 180°, Yaw 270° 15 = Roll 180°, Yaw 315° 16 = Roll 90° 17 = Roll 90°, Yaw 45° 18 = Roll 90°, Yaw 90° 19 = Roll 90°, Yaw 135° 20 = Roll 270° 21 = Roll 270°, Yaw 45° 22 = Roll 270°, Yaw 90° 23 = Roll 270°, Yaw 135° 24 = Pitch 90° 25 = Pitch 270° + + + PX4Flow board rotation + This parameter defines the rotation of the PX4FLOW board relative to the platform. Zero rotation is defined as Y on flow board pointing towards front of vehicle Possible values are: 0 = No rotation 1 = Yaw 45° 2 = Yaw 90° 3 = Yaw 135° 4 = Yaw 180° 5 = Yaw 225° 6 = Yaw 270° 7 = Yaw 315° + + + Board rotation Y (Pitch) offset + This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user to fine tune the board offset in the event of misalignment. + degrees + + + Board rotation X (Roll) offset + This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user to fine tune the board offset in the event of misalignment. + degrees + + + Board rotation Z (YAW) offset + This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user to fine tune the board offset in the event of misalignment. + degrees + + + External magnetometer rotation + This parameter defines the rotation of the external magnetometer relative to the platform (not relative to the FMU). See SENS_BOARD_ROT for possible values. + + + Set usage of external magnetometer + * Set to 0 (default) to auto-detect (will try to get the external as primary) * Set to 1 to force the external magnetometer as primary * Set to 2 to force the internal magnetometer as primary + 0 + 2 + + + + + Enable Lidar-Lite (LL40LS) pwm driver + 0 + 1 + + + + + Interval of one subscriber in the example in ms + + + Float Demonstration Parameter in the Example + + + + + Auto-start script index + CHANGING THIS VALUE REQUIRES A RESTART. Defines the auto-start script used to bootstrap the system. + + + Automatically configure default values + Set to 1 to reset parameters on next system startup (setting defaults). Platform-specific values are used if available. RC* parameters are preserved. + 0 + 1 + + + Set usage of IO board + Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. + 0 + 1 + + + Set restart type + Set by px4io to indicate type of restart + 0 + 2 + + + Companion computer interface + CHANGING THIS VALUE REQUIRES A RESTART. Configures the baud rate of the companion computer interface. Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!) 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1. 157600: enables OSD mode at 57600 baud, 8N1. + 0 + 921600 + + + Parameter version + This monotonically increasing number encodes the parameter compatibility set. whenever it increases parameters might not be backwards compatible and ground control stations should suggest a fresh configuration. + 0 + + + + + Enable UAVCAN + Allowed values: 0 - UAVCAN disabled. 1 - Enabled support for UAVCAN actuators and sensors. 2 - Enabled support for dynamic node ID allocation and firmware update. 3 - Sets the motor control outputs to UAVCAN and enables support for dynamic node ID allocation and firmware update. + 0 + 3 + + + UAVCAN Node ID + Read the specs at http://uavcan.org to learn more about Node ID. + 1 + 125 + + + UAVCAN CAN bus bitrate + 20000 + 1000000 + + + + + Position of tilt servo in mc mode + Position of tilt servo in mc mode + 0.0 + 1 + + + Position of tilt servo in transition mode + Position of tilt servo in transition mode + 0.0 + 1 + + + Position of tilt servo in fw mode + Position of tilt servo in fw mode + 0.0 + 1 + + + Duration of front transition phase 2 + Time in seconds it should take for the rotors to rotate forward completely from the point when the plane has picked up enough airspeed and is ready to go into fixed wind mode. + 0.1 + 2 + + + The channel number of motors that must be turned off in fixed wing mode + 0 + 123456 + + + Target throttle value for pusher/puller motor during the transition to fw mode + 0.0 + 1.0 + + + VTOL number of engines + 0 + + + Idle speed of VTOL when in multicopter mode + 900 + + + Minimum airspeed in multicopter mode + This is the minimum speed of the air flowing over the control surfaces. + 0.0 + + + Maximum airspeed in multicopter mode + This is the maximum speed of the air flowing over the control surfaces. + 0.0 + + + Trim airspeed when in multicopter mode + This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode. + 0.0 + + + Permanent stabilization in fw mode + If set to one this parameter will cause permanent attitude stabilization in fw mode. This parameter has been introduced for pure convenience sake. + 0 + 1 + + + Fixed wing pitch trim + This parameter allows to adjust the neutral elevon position in fixed wing mode. + -1 + 1 + + + Motor max power + Indicates the maximum power the motor is able to produce. Used to calculate propeller efficiency map. + 1 + + + Propeller efficiency parameter + Influences propeller efficiency at different power settings. Should be tuned beforehand. + 0.0 + 0.9 + + + Total airspeed estimate low-pass filter gain + Gain for tuning the low-pass filter for the total airspeed estimate + 0.0 + 0.99 + + + VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) + 0 + 2 + + + Lock elevons in multicopter mode + If set to 1 the elevons are locked in multicopter mode + 0 + 1 + + + Duration of a front transition + Time in seconds used for a transition + 0.0 + 5 + + + Duration of a back transition + Time in seconds used for a back transition + 0.0 + 5 + + + Transition blending airspeed + Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable. + 0.0 + 20.0 + + + Transition airspeed + Airspeed at which we can switch to fw mode + 1.0 + 20 + + + + + mTECS enabled + Set to 1 to enable mTECS + 0 + 1 + + + Total Energy Rate Control Feedforward + Maps the total energy rate setpoint to the throttle setpoint + 0.0 + 10.0 + + + Total Energy Rate Control P + Maps the total energy rate error to the throttle setpoint + 0.0 + 10.0 + + + Total Energy Rate Control I + Maps the integrated total energy rate to the throttle setpoint + 0.0 + 10.0 + + + Total Energy Rate Control Offset (Cruise throttle sp) + 0.0 + 10.0 + + + Energy Distribution Rate Control Feedforward + Maps the energy distribution rate setpoint to the pitch setpoint + 0.0 + 10.0 + + + Energy Distribution Rate Control P + Maps the energy distribution rate error to the pitch setpoint + 0.0 + 10.0 + + + Energy Distribution Rate Control I + Maps the integrated energy distribution rate error to the pitch setpoint + 0.0 + 10.0 + + + Total Energy Distribution Offset (Cruise pitch sp) + 0.0 + 10.0 + + + Minimal Throttle Setpoint + 0.0 + 1.0 + + + Maximal Throttle Setpoint + 0.0 + 1.0 + + + Minimal Pitch Setpoint in Degrees + -90.0 + 90.0 + deg + + + Maximal Pitch Setpoint in Degrees + -90.0 + 90.0 + deg + + + Lowpass (cutoff freq.) for altitude + + + Lowpass (cutoff freq.) for the flight path angle + + + P gain for the altitude control + Maps the altitude error to the flight path angle setpoint + 0.0 + 10.0 + + + D gain for the altitude control + Maps the change of altitude error to the flight path angle setpoint + 0.0 + 10.0 + + + Lowpass for FPA error derivative calculation (see MT_FPA_D) + + + Minimal flight path angle setpoint + -90.0 + 90.0 + deg + + + Maximal flight path angle setpoint + -90.0 + 90.0 + deg + + + Lowpass (cutoff freq.) for airspeed + + + Airspeed derivative calculation lowpass + + + P gain for the airspeed control + Maps the airspeed error to the acceleration setpoint + 0.0 + 10.0 + + + D gain for the airspeed control + Maps the change of airspeed error to the acceleration setpoint + 0.0 + 10.0 + + + Lowpass for ACC error derivative calculation (see MT_ACC_D) + + + Minimal acceleration (air) + m/s^2 + + + Maximal acceleration (air) + m/s^2 + + + Minimal throttle during takeoff + 0.0 + 1.0 + + + Maximal throttle during takeoff + 0.0 + 1.0 + + + Minimal pitch during takeoff + -90.0 + 90.0 + deg + + + Maximal pitch during takeoff + -90.0 + 90.0 + deg + + + Minimal throttle in underspeed mode + 0.0 + 1.0 + + + Maximal throttle in underspeed mode + 0.0 + 1.0 + + + Minimal pitch in underspeed mode + -90.0 + 90.0 + deg + + + Maximal pitch in underspeed mode + -90.0 + 90.0 + deg + + + Minimal throttle in landing mode (only last phase of landing) + 0.0 + 1.0 + + + Maximal throttle in landing mode (only last phase of landing) + 0.0 + 1.0 + + + Minimal pitch in landing mode + -90.0 + 90.0 + deg + + + Maximal pitch in landing mode + -90.0 + 90.0 + deg + + + Integrator Limit for Total Energy Rate Control + 0.0 + 10.0 + + + Integrator Limit for Energy Distribution Rate Control + 0.0 + 10.0 + + + + + RV_YAW_P + + + EXFW_HDNG_P + + + EXFW_ROLL_P + + + EXFW_PITCH_P + + + Failsafe channel mapping + The RC mapping index indicates which channel is used for failsafe If 0, whichever channel is mapped to throttle is used otherwise the value indicates the specific rc channel to use + 0 + 18 + + + TEST_MIN + + + TEST_MAX + + + TEST_TRIM + + + TEST_HP + + + TEST_LP + + + TEST_P + + + TEST_I + + + TEST_I_MAX + + + TEST_D + + + TEST_D_LP + + + TEST_MEAN + + + TEST_DEV + + + Flare, minimum pitch + Minimum pitch during flare, a positive sign means nose up Applied once FW_LND_TLALT is reached + + + Flare, maximum pitch + Maximum pitch during flare, a positive sign means nose up Applied once FW_LND_TLALT is reached + + + SEG_TH2V_P + + + SEG_TH2V_I + + + SEG_TH2V_I_MAX + + + SEG_Q2V + + + FWB_P_LP + + + FWB_Q_LP + + + FWB_R_LP + + + FWB_R_HP + + + FWB_P2AIL + + + FWB_Q2ELV + + + FWB_R2RDR + + + FWB_PSI2PHI + + + FWB_PHI2P + + + FWB_PHI_LIM_MAX + + + FWB_V2THE_P + + + FWB_V2THE_I + + + FWB_V2THE_D + + + FWB_V2THE_D_LP + + + FWB_V2THE_I_MAX + + + FWB_THE_MIN + + + FWB_THE_MAX + + + FWB_THE2Q_P + + + FWB_THE2Q_I + + + FWB_THE2Q_D + + + FWB_THE2Q_D_LP + + + FWB_THE2Q_I_MAX + + + FWB_H2THR_P + + + FWB_H2THR_I + + + FWB_H2THR_D + + + FWB_H2THR_D_LP + + + FWB_H2THR_I_MAX + + + FWB_XT2YAW_MAX + + + FWB_XT2YAW + + + FWB_V_MIN + + + FWB_V_CMD + + + FWB_V_MAX + + + FWB_CR_MAX + + + FWB_CR2THR_P + + + FWB_CR2THR_I + + + FWB_CR2THR_D + + + FWB_CR2THR_D_LP + + + FWB_CR2THR_I_MAX + + + FWB_TRIM_THR + + + FWB_TRIM_V + + + RGB Led brightness limit + Set to 0 to disable, 1 for minimum brightness up to 15 (max) + 0 + 15 + + + + \ No newline at end of file diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/px4/Px4Native.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/px4/Px4Native.java index ef9ef5974d..32f0759d82 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/px4/Px4Native.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/autopilot/px4/Px4Native.java @@ -1,15 +1,21 @@ package org.droidplanner.services.android.core.drone.autopilot.px4; import android.content.Context; +import android.os.Bundle; import android.os.Handler; import com.MAVLink.Messages.MAVLinkMessage; +import com.o3dr.services.android.lib.drone.action.ParameterActions; +import com.o3dr.services.android.lib.model.ICommandListener; +import com.o3dr.services.android.lib.model.action.Action; import org.droidplanner.services.android.communication.model.DataLink; import org.droidplanner.services.android.core.drone.LogMessageListener; import org.droidplanner.services.android.core.drone.autopilot.generic.GenericMavLinkDrone; +import org.droidplanner.services.android.core.drone.variables.HeartBeat; import org.droidplanner.services.android.core.firmware.FirmwareType; import org.droidplanner.services.android.core.model.AutopilotWarningParser; +import org.droidplanner.services.android.utils.CommonApiUtils; /** * Created by Fredia Huya-Kouadio on 9/10/15. @@ -25,4 +31,37 @@ public FirmwareType getFirmwareType() { return FirmwareType.PX4_NATIVE; } + @Override + protected HeartBeat initHeartBeat(Handler handler) { + return new HeartBeat(this, handler); + } + + + @Override + public boolean executeAsyncAction(Action action, ICommandListener listener) { + String type = action.getType(); + Bundle data = action.getData(); + if (data == null) { + data = new Bundle(); + } + switch (type) { + + case ParameterActions.ACTION_REFRESH_PARAMETERS: + CommonApiUtils.refreshParameters(this); + return true; + + default: + return super.executeAsyncAction(action, listener); + } + + } + + @Override + public void onMavLinkMessageReceived(MAVLinkMessage message) { + + getParameterManager().processMessage(message); + + super.onMavLinkMessageReceived(message); + } + } diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterManager.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterManager.java index 414fb7aeff..07c08a80bb 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterManager.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterManager.java @@ -3,11 +3,13 @@ import android.content.Context; import android.os.Handler; import android.text.TextUtils; +import android.util.Log; import android.util.SparseBooleanArray; import com.MAVLink.Messages.MAVLinkMessage; import com.MAVLink.common.msg_param_value; import com.o3dr.services.android.lib.drone.property.Parameter; +import com.o3dr.services.android.lib.drone.property.Type; import org.droidplanner.services.android.core.MAVLink.MavLinkParameters; import org.droidplanner.services.android.core.drone.DroneInterfaces; @@ -15,7 +17,9 @@ import org.droidplanner.services.android.core.drone.DroneInterfaces.OnDroneListener; import org.droidplanner.services.android.core.drone.DroneVariable; import org.droidplanner.services.android.core.drone.autopilot.MavLinkDrone; +import org.droidplanner.services.android.core.firmware.FirmwareType; import org.droidplanner.services.android.utils.file.IO.ParameterMetadataLoader; +import org.droidplanner.services.android.utils.file.IO.ParameterMetadataLoaderPX4; import java.util.Locale; import java.util.Map; @@ -64,12 +68,15 @@ public void run() { private final SparseBooleanArray paramsRollCall = new SparseBooleanArray(); private final ConcurrentHashMap parameters = new ConcurrentHashMap<>(); private final ConcurrentHashMap parametersMetadata = new ConcurrentHashMap<>(); + private final ConcurrentHashMap parametersMetadataPX4 = new ConcurrentHashMap<>(); private DroneInterfaces.OnParameterManagerListener parameterListener; private final Handler watchdog; private final Context context; + private String metadataType; + public ParameterManager(MavLinkDrone myDrone, Context context, Handler handler) { super(myDrone); this.context = context; @@ -84,7 +91,7 @@ public void refreshParameters() { parameters.clear(); paramsRollCall.clear(); - notifyParametersReceiptStart(); + notifyParametersReceiptStart(); // Tower --> LocalBroadcast --> PARAMETERS_REFRESH_STARTED MavLinkParameters.requestParametersList(myDrone); resetWatchdog(); @@ -108,8 +115,11 @@ public Map getParameters() { */ public boolean processMessage(MAVLinkMessage msg) { if (msg.msgid == msg_param_value.MAVLINK_MSG_ID_PARAM_VALUE) { + Log.d("kiba", "processMessage: msg.msgid == msg_param_value.MAVLINK_MSG_ID_PARAM_VALUE? " + true); processReceivedParam((msg_param_value) msg); return true; + }else{ + Log.d("kiba", "processMessage: msg.msgid == msg_param_value.MAVLINK_MSG_ID_PARAM_VALUE? " + false); } return false; } @@ -120,12 +130,13 @@ protected void processReceivedParam(msg_param_value m_value) { loadParameterMetadata(param); parameters.put(param.getName().toLowerCase(Locale.US), param); + Log.d("kiba", "processReceivedParam: parameters size = " + parameters.size()); int paramIndex = m_value.param_index; if (paramIndex == -1) { // update listener notifyParameterReceipt(param, 0, 1); - notifyParametersReceiptEnd(); + notifyParametersReceiptEnd(); // Tower --> PARAMETERS_REFRESH_COMPLETED return; } @@ -140,7 +151,7 @@ protected void processReceivedParam(msg_param_value m_value) { killWatchdog(); isRefreshing.set(false); - notifyParametersReceiptEnd(); + notifyParametersReceiptEnd(); //Tower --> PARAMETERS_REFRESH_COMPLETED } else { resetWatchdog(); } @@ -213,14 +224,20 @@ public void onDroneEvent(DroneEventsType event, MavLinkDrone drone) { private void refreshParametersMetadata() { //Reload the vehicle parameters metadata String metadataType = myDrone.getFirmwareType().getParameterMetadataGroup(); + this.metadataType = metadataType; if (!TextUtils.isEmpty(metadataType)) { try { - ParameterMetadataLoader.load(context, metadataType, this.parametersMetadata); + if(metadataType.equals(FirmwareType.PX4_NATIVE.getParameterMetadataGroup())){ + ParameterMetadataLoaderPX4.load(context, metadataType, this.parametersMetadataPX4); + Log.d("kiba", "parseMetadataPX4: size = " + parametersMetadataPX4.size()); + }else{ + ParameterMetadataLoader.load(context, metadataType, this.parametersMetadata); + Log.d("kiba", "parseMetadata: size = " + parametersMetadata.size()); + } } catch (Exception e) { Timber.e(e, e.getMessage()); } } - if (parametersMetadata.isEmpty() || parameters.isEmpty()) return; @@ -230,14 +247,26 @@ private void refreshParametersMetadata() { } private void loadParameterMetadata(Parameter parameter){ - ParameterMetadata metadata = parametersMetadata.get(parameter.getName()); - if (metadata != null) { - parameter.setDisplayName(metadata.getDisplayName()); - parameter.setDescription(metadata.getDescription()); - parameter.setUnits(metadata.getUnits()); - parameter.setRange(metadata.getRange()); - parameter.setValues(metadata.getValues()); + if(metadataType.equals(FirmwareType.PX4_NATIVE.getParameterMetadataGroup())){ // PX4 + ParameterMetadataPX4 metadata = parametersMetadataPX4.get(parameter.getName()); + if (metadata != null) { + parameter.setDisplayName(metadata.getDisplayName()); + parameter.setDescription(metadata.getDescription()); + parameter.setUnits(metadata.getUnits()); + parameter.setRange(metadata.getRange()); + parameter.setValues(metadata.getValues()); + } + }else{ // ArduPilot + ParameterMetadata metadata = parametersMetadata.get(parameter.getName()); + if (metadata != null) { + parameter.setDisplayName(metadata.getDisplayName()); + parameter.setDescription(metadata.getDescription()); + parameter.setUnits(metadata.getUnits()); + parameter.setRange(metadata.getRange()); + parameter.setValues(metadata.getValues()); + } } + } public void setParameterListener(DroneInterfaces.OnParameterManagerListener parameterListener) { diff --git a/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterMetadataPX4.java b/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterMetadataPX4.java new file mode 100644 index 0000000000..44fbe09f38 --- /dev/null +++ b/ServiceApp/src/org/droidplanner/services/android/core/drone/profiles/ParameterMetadataPX4.java @@ -0,0 +1,75 @@ +package org.droidplanner.services.android.core.drone.profiles; + +/** + * Created by KiBa on 2015/12/21. + */ +public class ParameterMetadataPX4 extends ParameterMetadata{ + + private String decimal; + private String groupName; + private String defaultAttr; + private String typeAttr; + private String max; + private String min; + + public String getDecimal() { + return decimal; + } + + public String getGroupName() { + return groupName; + } + + public String getMax() { + return max; + } + + public String getMin() { + return min; + } + + public String getDefaultAttr() { + return defaultAttr; + } + + public String getTypeAttr() { + return typeAttr; + } + + public void setDecimal(String decimal) { + this.decimal = decimal; + } + + public void setGroupName(String groupName) { + this.groupName = groupName; + } + + public void setDefaultAttr(String defaultAttr) { + this.defaultAttr = defaultAttr; + } + + public void setTypeAttr(String typeAttr) { + this.typeAttr = typeAttr; + } + + public void setMax(String max) { + this.max = max; + } + + public void setMin(String min) { + this.min = min; + } + + public String toString(){ + String str = " group = " + getGroupName() + + "\n name = " + getName() + + "\n default = " + getDefaultAttr() + + "\n type = " + getTypeAttr() + + "\n display = " + getDisplayName() + + "\n description = " + getDescription() + + "\n unit = " + getUnits() + + "\n range = " + getRange() + + "\n values = " + getValues(); + return str; + } +} diff --git a/ServiceApp/src/org/droidplanner/services/android/core/firmware/FirmwareType.java b/ServiceApp/src/org/droidplanner/services/android/core/firmware/FirmwareType.java index 4069d10183..521fa36eb3 100644 --- a/ServiceApp/src/org/droidplanner/services/android/core/firmware/FirmwareType.java +++ b/ServiceApp/src/org/droidplanner/services/android/core/firmware/FirmwareType.java @@ -12,7 +12,7 @@ public enum FirmwareType { /** * PX4 firmware type */ - PX4_NATIVE(MAV_AUTOPILOT.MAV_AUTOPILOT_PX4, "", "PX4 Native"), + PX4_NATIVE(MAV_AUTOPILOT.MAV_AUTOPILOT_PX4, "PX4", "PX4 Native"), /** * Generic firmware type diff --git a/ServiceApp/src/org/droidplanner/services/android/utils/file/IO/ParameterMetadataLoaderPX4.java b/ServiceApp/src/org/droidplanner/services/android/utils/file/IO/ParameterMetadataLoaderPX4.java new file mode 100644 index 0000000000..59106ebfb8 --- /dev/null +++ b/ServiceApp/src/org/droidplanner/services/android/utils/file/IO/ParameterMetadataLoaderPX4.java @@ -0,0 +1,205 @@ +package org.droidplanner.services.android.utils.file.IO; + +import android.content.Context; +import android.text.TextUtils; +import android.util.Xml; + +import org.droidplanner.services.android.core.drone.profiles.ParameterMetadataPX4; +import org.xmlpull.v1.XmlPullParser; +import org.xmlpull.v1.XmlPullParserException; + +import java.io.IOException; +import java.io.InputStream; +import java.util.ArrayList; +import java.util.List; +import java.util.Map; + +/** + * Created by KiBa on 2015/12/24. + */ +public class ParameterMetadataLoaderPX4 { + + private static final String PARAMETERMETADATA_PATH = "Parameters/ParameterMetaData.xml"; + + //**********************PX4 metadata************************ + private static final String METADATA_GROUP_PX4 = "group"; + private static final String METADATA_PARAMETER_PX4 = "parameter"; + + private static final String METADATA_DISPLAYNAME_PX4 = "short_desc"; + private static final String METADATA_DESCRIPTION_PX4 = "long_desc"; + private static final String METADATA_UNITS_PX4 = "unit"; + private static final String METADATA_RANGE_MAX_PX4 = "max"; + private static final String METADATA_RANGE_MIN_PX4 = "min"; + private static final String METADATA_DECIMAL_PX4 = "decimal"; + + private static final String METADATA_TYPE_PX4_FLOAT = "FLOAT"; + private static final String METADATA_TYPE_PX4_INT32 = "INT32"; + private static List minListInt32 = new ArrayList<>(); + private static List minListFloat = new ArrayList<>(); + + public static void load(Context context, String metadataType, Map metadata) + throws IOException, XmlPullParserException { + InputStream inputStream = context.getAssets().open(PARAMETERMETADATA_PATH); + open(inputStream, metadataType, metadata); + } + + private static void open(InputStream inputStream, String metadataType, Map metadataMap) + throws XmlPullParserException, IOException { + try { + XmlPullParser parser = Xml.newPullParser(); + parser.setFeature(XmlPullParser.FEATURE_PROCESS_NAMESPACES, false); + parser.setInput(inputStream, null); + parseMetadataPX4(parser, metadataType, metadataMap); + + } finally { + try { + inputStream.close(); + } catch (IOException e) { /* nop */ + } + } + } + + //**************************** PX4 ********************************* + + private static void parseMetadataPX4(XmlPullParser pullParser, String metadataType, + Map metadataMap) throws XmlPullParserException, IOException { + int type = pullParser.getEventType(); + + ParameterMetadataPX4 metadataPX4 = null; + String name = null; // label name + String groupName = null; // group--'name' attribute + String defaultAttr = null; // parameter--'default' attribute + String nameAttr = null; // parameter--'name' attribute + String typeAttr = null; // parameter--'type' attribute + + boolean parsing = false; + + while (type != XmlPullParser.END_DOCUMENT) { + switch (type) { + case XmlPullParser.START_TAG: + name = pullParser.getName(); // label name + + if (TextUtils.equals(metadataType, name)) { + parsing = true; + } else if (parsing) { + // if the label name is + if (TextUtils.equals(pullParser.getName(), METADATA_GROUP_PX4)) { + groupName = pullParser.getAttributeValue(0); + } else if (TextUtils.equals(pullParser.getName(), METADATA_PARAMETER_PX4)) { // if label name isn't 'group' + metadataPX4 = new ParameterMetadataPX4(); + // get attributes of the label + if (groupName != null) { + metadataPX4.setGroupName(groupName); // group value + } + defaultAttr = pullParser.getAttributeValue(0); // default value + nameAttr = pullParser.getAttributeValue(1); // name value + typeAttr = pullParser.getAttributeValue(2); // type value + // set attributes to ParameterMetadataPX4 + metadataPX4.setDefaultAttr(defaultAttr); + metadataPX4.setName(nameAttr); + metadataPX4.setTypeAttr(typeAttr); + + } else { + if (metadataPX4 != null) { + addMetaDataPropertyPX4(metadataPX4, name, pullParser.nextText()); + } + } + } + break; + + case XmlPullParser.END_TAG: + if (TextUtils.equals(metadataType, name)) { + return; + } else if (metadataPX4 != null && metadataPX4.getName().equals(nameAttr)) { + metadataMap.put(metadataPX4.getName(), metadataPX4); + metadataPX4 = null; + } + break; + default: + break; + } + + type = pullParser.next(); + } + } + + + private static void addMetaDataPropertyPX4(ParameterMetadataPX4 metaData, String labelName, String text) { + switch (labelName) { + case METADATA_DISPLAYNAME_PX4: + metaData.setDisplayName(text); + break; + case METADATA_DESCRIPTION_PX4: + metaData.setDescription(text); + break; + case METADATA_UNITS_PX4: + metaData.setUnits(text); + break; + case METADATA_DECIMAL_PX4: + metaData.setDecimal(text); + break; + case METADATA_RANGE_MIN_PX4: + handleMinRange(metaData, text); + break; + case METADATA_RANGE_MAX_PX4: + handleMaxRange(metaData, text); + break; + + } + } + + private static void handleMinRange(ParameterMetadataPX4 metadataPX4, String text) { + // if the type is int32, it indicates that it can be converted to 'values' + if (TextUtils.equals(metadataPX4.getTypeAttr(), METADATA_TYPE_PX4_INT32)) { + if (minListInt32.isEmpty()) { + minListInt32.add(0, Integer.parseInt(text)); + } + } + // if the type is float, it indicates that it only can be converted to 'range' + else if (TextUtils.equals(metadataPX4.getTypeAttr(), METADATA_TYPE_PX4_FLOAT)) { + if (minListFloat.isEmpty()) { + minListFloat.add(0, Float.parseFloat(text)); + } + } + metadataPX4.setMin(text); + } + + private static void handleMaxRange(ParameterMetadataPX4 metadataPX4, String text) { + // set max value + metadataPX4.setMax(text); + // if the type is int32, it indicates that it can be converted to 'values' + if (TextUtils.equals(metadataPX4.getTypeAttr(), METADATA_TYPE_PX4_INT32)) { + if (!minListInt32.isEmpty()) { + int min = minListInt32.get(0); + int max = Integer.parseInt(text); + // if the diff is between 2, that can be chose, else fill in manually + if (min == 0 || min == -1) { + if (max - min <= 2) { + String value = ""; + for (int i = min; i <= max; i++) { + if (i != max) { + value = value + i + ": ,"; + } else { + value = value + i + ": "; + } + } + metadataPX4.setValues(value); + } + } else { + String range = min + " " + max; + metadataPX4.setRange(range); + } + minListInt32.clear(); + } + } + // if the type is float, it indicates that it only can be converted to 'range' + else if (TextUtils.equals(metadataPX4.getTypeAttr(), METADATA_TYPE_PX4_FLOAT)) { + if (!minListFloat.isEmpty()) { + float min = minListFloat.get(0); + String range = min + " " + text; + metadataPX4.setRange(range); + } + minListFloat.clear(); + } + } +} \ No newline at end of file