package com.o3dr.android.client.apis;
import android.os.Bundle;
import android.support.annotation.NonNull;
import com.MAVLink.enums.MAV_MOUNT_MODE;
import com.o3dr.android.client.Drone;
import com.o3dr.android.client.interfaces.DroneListener;
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.drone.attribute.error.CommandExecutionError;
import com.o3dr.services.android.lib.drone.property.Type;
import com.o3dr.services.android.lib.model.SimpleCommandListener;
import com.o3dr.services.android.lib.model.action.Action;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.ConcurrentLinkedQueue;
import static com.o3dr.services.android.lib.drone.action.GimbalActions.ACTION_SET_GIMBAL_MOUNT_MODE;
import static com.o3dr.services.android.lib.drone.action.GimbalActions.ACTION_SET_GIMBAL_ORIENTATION;
import static com.o3dr.services.android.lib.drone.action.GimbalActions.GIMBAL_MOUNT_MODE;
import static com.o3dr.services.android.lib.drone.action.GimbalActions.GIMBAL_PITCH;
import static com.o3dr.services.android.lib.drone.action.GimbalActions.GIMBAL_ROLL;
import static com.o3dr.services.android.lib.drone.action.GimbalActions.GIMBAL_YAW;
public final class GimbalApi extends Api implements DroneListener {
private static final ConcurrentHashMap<Drone, GimbalApi> gimbalApiCache = new ConcurrentHashMap<>();
private static final Builder<GimbalApi> apiBuilder = new Builder<GimbalApi>() {
@Override
public GimbalApi build(Drone drone) {
return new GimbalApi(drone);
}
};
public static GimbalApi getApi(final Drone drone){
return getApi(drone, gimbalApiCache, apiBuilder);
}
public interface GimbalOrientationListener {
/**
* Called when the gimbal orientation is updated.
* @param orientation GimbalOrientation object
*/
void onGimbalOrientationUpdate(GimbalOrientation orientation);
/**
* Indicates errors occurring from attempting to set the gimbal orientation.
* @param error @see {@link com.o3dr.services.android.lib.drone.attribute.error.CommandExecutionError}
*/
void onGimbalOrientationCommandError(int error);
}
/**
* Stores the gimbal orientation angles.
*/
public static class GimbalOrientation {
private float pitch;
private float roll;
private float yaw;
public float getPitch() {
return pitch;
}
public float getRoll() {
return roll;
}
public float getYaw() {
return yaw;
}
private void updateOrientation(float pitch, float roll, float yaw) {
this.pitch = pitch;
this.roll = roll;
this.yaw = yaw;
}
private GimbalOrientation(){}
private GimbalOrientation(GimbalOrientation source){
this.pitch = source.pitch;
this.roll = source.roll;
this.yaw = source.yaw;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (!(o instanceof GimbalOrientation)) return false;
GimbalOrientation that = (GimbalOrientation) o;
if (Float.compare(that.pitch, pitch) != 0) return false;
if (Float.compare(that.roll, roll) != 0) return false;
return Float.compare(that.yaw, yaw) == 0;
}
@Override
public int hashCode() {
int result = (pitch != +0.0f ? Float.floatToIntBits(pitch) : 0);
result = 31 * result + (roll != +0.0f ? Float.floatToIntBits(roll) : 0);
result = 31 * result + (yaw != +0.0f ? Float.floatToIntBits(yaw) : 0);
return result;
}
@Override
public String toString() {
return "GimbalOrientation{" +
"pitch=" + pitch +
", roll=" + roll +
", yaw=" + yaw +
'}';
}
}
private final ConcurrentLinkedQueue<GimbalOrientationListener> gimbalListeners = new ConcurrentLinkedQueue<>();
private final Drone drone;
private final GimbalOrientation gimbalOrientation = new GimbalOrientation();
private GimbalApi(Drone drone){
this.drone = drone;
this.drone.registerDroneListener(this);
}
public GimbalOrientation getGimbalOrientation(){
return new GimbalOrientation(gimbalOrientation);
}
/**
* Enables control of the gimbal. After calling this method, use {@link GimbalApi#updateGimbalOrientation(float, float, float, GimbalOrientationListener)}
* to update the gimbal orientation.
* @param listener non-null GimbalStatusListener callback.
*/
public void startGimbalControl(final GimbalOrientationListener listener){
if(listener == null)
throw new NullPointerException("Listener can't be null.");
final Type vehicleType = drone.getAttribute(AttributeType.TYPE);
if(vehicleType.getDroneType() != Type.TYPE_COPTER){
drone.post(new Runnable() {
@Override
public void run() {
listener.onGimbalOrientationCommandError(CommandExecutionError.COMMAND_UNSUPPORTED);
}
});
return;
}
gimbalListeners.add(listener);
configureGimbalMountMode(listener);
}
private void configureGimbalMountMode(final GimbalOrientationListener listener){
Bundle params = new Bundle(1);
params.putInt(GIMBAL_MOUNT_MODE, MAV_MOUNT_MODE.MAV_MOUNT_MODE_MAVLINK_TARGETING);
drone.performAsyncActionOnDroneThread(new Action(ACTION_SET_GIMBAL_MOUNT_MODE, params), new SimpleCommandListener() {
@Override
public void onTimeout() {
listener.onGimbalOrientationCommandError(CommandExecutionError.COMMAND_FAILED);
}
@Override
public void onError(int error) {
listener.onGimbalOrientationCommandError(error);
}
});
}
/**
* Disables control of the gimbal. After calling this method, no call to {@link GimbalApi#updateGimbalOrientation(float, float, float, GimbalOrientationListener)}
* will be allowed.
* @param listener non-null GimbalStatusListener callback.
*
* @since 2.5.0
*/
public void stopGimbalControl(final GimbalOrientationListener listener){
if(listener == null)
throw new NullPointerException("Listener can't be null.");
if(!gimbalListeners.contains(listener)){
drone.post(new Runnable() {
@Override
public void run() {
listener.onGimbalOrientationCommandError(CommandExecutionError.COMMAND_DENIED);
}
});
return;
}
gimbalListeners.remove(listener);
//Reset the gimbal mount to the default.
Bundle params = new Bundle(1);
params.putInt(GIMBAL_MOUNT_MODE, MAV_MOUNT_MODE.MAV_MOUNT_MODE_RC_TARGETING);
drone.performAsyncActionOnDroneThread(new Action(ACTION_SET_GIMBAL_MOUNT_MODE, params), new SimpleCommandListener(){
@Override
public void onTimeout(){
listener.onGimbalOrientationCommandError(CommandExecutionError.COMMAND_FAILED);
}
@Override
public void onError(int error){
listener.onGimbalOrientationCommandError(error);
}
});
}
/**
* Set the orientation of the gimbal
* @param orientation Desired orientation values.
* @param listener Register a callback to receive update of the command execution state. Must be non-null.
* @since 2.8.0
*/
public void updateGimbalOrientation(GimbalOrientation orientation, @NonNull final GimbalOrientationListener listener) {
updateGimbalOrientation(orientation.pitch, orientation.roll, orientation.yaw, listener);
}
/**
* Set the orientation of a gimbal
*
* @param pitch the desired gimbal pitch in degrees. 0 is straight forwards, -90 is straight down
* @param roll the desired gimbal roll in degrees
* @param yaw the desired gimbal yaw in degrees
* @param listener Register a callback to receive update of the command execution state. Must be non-null.
* @since 2.5.0
*/
public void updateGimbalOrientation(float pitch, float roll, float yaw, @NonNull final GimbalOrientationListener listener){
if(listener == null)
throw new NullPointerException("Listener must be non-null.");
if(!gimbalListeners.contains(listener)){
drone.post(new Runnable() {
@Override
public void run() {
listener.onGimbalOrientationCommandError(CommandExecutionError.COMMAND_DENIED);
}
});
return;
}
Bundle params = new Bundle();
params.putFloat(GIMBAL_PITCH, pitch);
params.putFloat(GIMBAL_ROLL, roll);
params.putFloat(GIMBAL_YAW, yaw);
drone.performAsyncActionOnDroneThread(new Action(ACTION_SET_GIMBAL_ORIENTATION, params), new SimpleCommandListener(){
@Override
public void onTimeout(){
listener.onGimbalOrientationCommandError(CommandExecutionError.COMMAND_FAILED);
}
@Override
public void onError(int error){
listener.onGimbalOrientationCommandError(error);
}
});
}
private void notifyGimbalOrientationUpdated(GimbalOrientation orientation){
if(gimbalListeners.isEmpty())
return;
for(GimbalOrientationListener listener: gimbalListeners){
listener.onGimbalOrientationUpdate(orientation);
}
}
@Override
public void onDroneEvent(String event, Bundle extras) {
switch(event){
case AttributeEvent.GIMBAL_ORIENTATION_UPDATED:
final float pitch = extras.getFloat(AttributeEventExtra.EXTRA_GIMBAL_ORIENTATION_PITCH);
final float roll = extras.getFloat(AttributeEventExtra.EXTRA_GIMBAL_ORIENTATION_ROLL);
final float yaw = extras.getFloat(AttributeEventExtra.EXTRA_GIMBAL_ORIENTATION_YAW);
gimbalOrientation.updateOrientation(pitch, roll, yaw);
notifyGimbalOrientationUpdated(gimbalOrientation);
break;
}
}
@Override
public void onDroneServiceInterrupted(String errorMsg) {
}
}