Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Follow UI improvements #1124

Merged
merged 6 commits into from
Oct 3, 2014
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import org.droidplanner.core.model.Drone;

import android.app.Activity;
import android.graphics.Color;
import android.os.Bundle;
import android.support.v4.app.Fragment;
import android.view.LayoutInflater;
Expand Down Expand Up @@ -156,6 +157,9 @@ public void onClick(View v) {
break;

case R.id.mc_pause:
if (followMe.isEnabled()) {
followMe.toggleFollowMeState();
}
drone.getGuidedPoint().pauseAtCurrentLocation();
eventBuilder.setAction("Changed flight mode").setLabel("Pause");
break;
Expand All @@ -172,26 +176,30 @@ public void onClick(View v) {
break;

case R.id.mc_follow:
final int result = followMe.toggleFollowMeState();
followMe.toggleFollowMeState();
String eventLabel = null;
switch (result) {
case Follow.FOLLOW_START:
switch (followMe.getState()) {
case FOLLOW_START:
eventLabel = "FollowMe enabled";
break;

case FOLLOW_RUNNING:
eventLabel = "FollowMe running";
break;

case Follow.FOLLOW_END:
case FOLLOW_END:
eventLabel = "FollowMe disabled";
break;

case Follow.FOLLOW_INVALID_STATE:
case FOLLOW_INVALID_STATE:
eventLabel = "FollowMe error: invalid state";
break;

case Follow.FOLLOW_DRONE_DISCONNECTED:
case FOLLOW_DRONE_DISCONNECTED:
eventLabel = "FollowMe error: drone not connected";
break;

case Follow.FOLLOW_DRONE_NOT_ARMED:
case FOLLOW_DRONE_NOT_ARMED:
eventLabel = "FollowMe error: drone not armed";
break;
}
Expand Down Expand Up @@ -249,6 +257,8 @@ public void onDroneEvent(DroneEventsType event, Drone drone) {

case FOLLOW_START:
case FOLLOW_STOP:
case FOLLOW_UPDATE:
updateFlightModeButtons();
updateFollowButton();
break;

Expand All @@ -267,7 +277,7 @@ private void updateFlightModeButtons() {
break;

case ROTOR_GUIDED:
if (drone.getGuidedPoint().isIdle()) {
if (drone.getGuidedPoint().isIdle() && !followMe.isEnabled()) {
pauseBtn.setActivated(true);
}
break;
Expand All @@ -292,7 +302,19 @@ private void resetFlightModeButtons() {
}

private void updateFollowButton() {
followBtn.setActivated(followMe.isEnabled());
switch (followMe.getState()) {
case FOLLOW_START:
followBtn.setBackgroundColor(Color.RED);
break;
case FOLLOW_RUNNING:
followBtn.setActivated(true);
followBtn.setBackgroundResource(R.drawable.flight_action_row_bg_selector);
break;
default:
followBtn.setActivated(false);
followBtn.setBackgroundResource(R.drawable.flight_action_row_bg_selector);
break;
}
}

private void resetButtonsContainerVisibility() {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ public void onDroneEvent(DroneInterfaces.DroneEventsType event, Drone drone) {
case DISCONNECTED:
case MODE:
case TYPE:
case FOLLOW_START:
case FOLLOW_STOP:
// Update the mode info panel
onModeUpdate(drone.getState().getMode());
break;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -150,26 +150,27 @@ public void receiveData(Context context, int transactionId, PebbleDictionary dat
switch (request) {

case KEY_REQUEST_MODE_FOLLOW:
final int result = followMe.toggleFollowMeState();
followMe.toggleFollowMeState();
String eventLabel = null;
switch (result) {
case Follow.FOLLOW_START:
switch (followMe.getState()) {
case FOLLOW_START:
case FOLLOW_RUNNING:
eventLabel = "FollowMe enabled";
break;

case Follow.FOLLOW_END:
case FOLLOW_END:
eventLabel = "FollowMe disabled";
break;

case Follow.FOLLOW_INVALID_STATE:
case FOLLOW_INVALID_STATE:
eventLabel = "FollowMe error: invalid state";
break;

case Follow.FOLLOW_DRONE_DISCONNECTED:
case FOLLOW_DRONE_DISCONNECTED:
eventLabel = "FollowMe error: drone not connected";
break;

case Follow.FOLLOW_DRONE_NOT_ARMED:
case FOLLOW_DRONE_NOT_ARMED:
eventLabel = "FollowMe error: drone not armed";
break;
}
Expand All @@ -184,8 +185,7 @@ public void receiveData(Context context, int transactionId, PebbleDictionary dat
break;

case KEY_REQUEST_MODE_LOITER:
((DroidPlannerApp) applicationContext).getDrone().getState()
.changeFlightMode(ApmModes.ROTOR_LOITER);
((DroidPlannerApp) applicationContext).getDrone().getGuidedPoint().pauseAtCurrentLocation();
break;

case KEY_REQUEST_MODE_RTL:
Expand Down
5 changes: 5 additions & 0 deletions Core/src/org/droidplanner/core/drone/DroneInterfaces.java
Original file line number Diff line number Diff line change
Expand Up @@ -181,6 +181,11 @@ public enum DroneEventsType {
* 'Follow' mode has been disabled.
*/
FOLLOW_STOP,

/**
* 'Follow' state has been updated.
*/
FOLLOW_UPDATE,

/**
*
Expand Down
45 changes: 26 additions & 19 deletions Core/src/org/droidplanner/core/gcs/follow/Follow.java
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,12 @@

public class Follow implements OnDroneListener, LocationReceiver {

// Set of return value for the 'toggleFollowMeState' method.
public static final int FOLLOW_INVALID_STATE = -1;
public static final int FOLLOW_DRONE_NOT_ARMED = -2;
public static final int FOLLOW_DRONE_DISCONNECTED = -3;
public static final int FOLLOW_START = 0;
public static final int FOLLOW_END = 1;

private boolean followMeEnabled = false;
/** Set of return value for the 'toggleFollowMeState' method.*/
public enum FollowStates {
FOLLOW_INVALID_STATE, FOLLOW_DRONE_NOT_ARMED, FOLLOW_DRONE_DISCONNECTED, FOLLOW_START, FOLLOW_RUNNING, FOLLOW_END,
};

private FollowStates state = FollowStates.FOLLOW_INVALID_STATE;
private Drone drone;

private ROIEstimator roiEstimator;
Expand All @@ -40,48 +38,49 @@ public Follow(Drone drone, Handler handler, LocationFinder locationFinder) {
drone.addDroneListener(this);
}

public int toggleFollowMeState() {
public void toggleFollowMeState() {
final State droneState = drone.getState();
if (droneState == null) {
return FOLLOW_INVALID_STATE;
state = FollowStates.FOLLOW_INVALID_STATE;
return;
}

if (isEnabled()) {
disableFollowMe();
drone.getState().changeFlightMode(ApmModes.ROTOR_LOITER);
return FOLLOW_END;
} else {
if (drone.getMavClient().isConnected()) {
if (drone.getState().isArmed()) {
drone.getState().changeFlightMode(ApmModes.ROTOR_GUIDED);
enableFollowMe();
return FOLLOW_START;
} else {
return FOLLOW_DRONE_NOT_ARMED;
state = FollowStates.FOLLOW_DRONE_NOT_ARMED;
}
} else {
return FOLLOW_DRONE_DISCONNECTED;
state = FollowStates.FOLLOW_DRONE_DISCONNECTED;

}
}
return;
}

private void enableFollowMe() {
locationFinder.enableLocationUpdates();
followMeEnabled = true;
state = FollowStates.FOLLOW_START;
drone.notifyDroneEvent(DroneEventsType.FOLLOW_START);
}

private void disableFollowMe() {
locationFinder.disableLocationUpdates();
if (followMeEnabled) {
followMeEnabled = false;
if (isEnabled()) {
state = FollowStates.FOLLOW_END;
MavLinkROI.resetROI(drone);
drone.getGuidedPoint().pauseAtCurrentLocation();
drone.notifyDroneEvent(DroneEventsType.FOLLOW_STOP);
}
}

public boolean isEnabled() {
return followMeEnabled;
return state == FollowStates.FOLLOW_RUNNING || state == FollowStates.FOLLOW_START;
}

@Override
Expand All @@ -106,8 +105,12 @@ public Length getRadius() {
@Override
public void onLocationChanged(Location location) {
if (location.getAccuracy() < 10.0) {
state = FollowStates.FOLLOW_RUNNING;
followAlgorithm.processNewLocation(location);
}else{
state = FollowStates.FOLLOW_START;
}
drone.notifyDroneEvent(DroneEventsType.FOLLOW_UPDATE);
roiEstimator.onLocationChanged(location);
}

Expand All @@ -127,4 +130,8 @@ public void cycleType() {
public FollowModes getType() {
return followAlgorithm.getType();
}

public FollowStates getState() {
return state;
}
}