Fix motion bugs
- Fix landing sequence interruption (timerIgnoreMavLink) - Fix MAV_CMD_NAV_TAKEOFF response (for MAVSDK compatibility) - Fix rotation() calculation - Fix yaw units - Add isFlying to DummyProduct - Call landing callback in DummyProduct
Esse commit está contido em:
Christopher Pereira
@@ -1274,9 +1274,9 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
|
||||
int blockSeconds = 5;
|
||||
Log.i(TAG, "RC joystick moved => cancelling all tasks and blocking MAVLink motion for " + blockSeconds + " seconds");
|
||||
timerIgnoreMavLink = System.currentTimeMillis() + blockSeconds * 1000;
|
||||
cancelAllTasks();
|
||||
setVirtualSticksEnabled(false);
|
||||
timerIgnoreMavLink = System.currentTimeMillis() + blockSeconds * 1000;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1892,7 +1892,7 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
Log.i(TAG, "Starting take off...");
|
||||
|
||||
if(useMissionControlClass) {
|
||||
// TODO: Why do we want to start/resume a mission after taking off?. Should we start a mission when setting mode to auto (like Mission Planner)?
|
||||
// TODO: Remove this code? Why do we want to start/resume a WaypointMission after taking off?. This is not part of MAVLink spec.
|
||||
if (getWaypointMissionOperator().getCurrentState() == WaypointMissionState.EXECUTION_PAUSED) {
|
||||
resumeWaypointMission();
|
||||
}
|
||||
@@ -1912,8 +1912,11 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
|
||||
} else {
|
||||
FlightControllerState coord = mFlightController.getState();
|
||||
// TODO: BUG: Sometimes isFlying() returns true even when not flying.
|
||||
if(coord.isFlying()) {
|
||||
send_text("Already flying");
|
||||
Log.i(TAG, "doTakeOff(): already flying => ignore takeoff and only ascend");
|
||||
if(sendResponses) send_command_ack(MAV_CMD_NAV_TAKEOFF, MAV_RESULT.MAV_RESULT_ACCEPTED);
|
||||
ascend(alt);
|
||||
|
||||
} else {
|
||||
@@ -1921,7 +1924,9 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
@Override
|
||||
public void onResult(DJIError djiError) {
|
||||
if (djiError == null) {
|
||||
if(sendResponses) send_command_ack(MAV_CMD_NAV_TAKEOFF, MAV_RESULT.MAV_RESULT_ACCEPTED); // Works with MAVSDK
|
||||
ascend(alt);
|
||||
|
||||
} else {
|
||||
Log.e(TAG, "startTakeoff() error: " + djiError.toString());
|
||||
|
||||
@@ -1965,8 +1970,12 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
}
|
||||
|
||||
void doLand() {
|
||||
parent.logMessageDJI("Landing...");
|
||||
if(System.currentTimeMillis() < timerIgnoreMavLink) return; // Prevent landing again if already landing.
|
||||
|
||||
timerIgnoreMavLink = System.currentTimeMillis() + 5 * 1000;
|
||||
cancelAllTasks();
|
||||
|
||||
parent.logMessageDJI("Landing...");
|
||||
//setMavFlightMode(ArduCopterFlightModes.LAND);
|
||||
mFlightController.startLanding(djiError -> {
|
||||
if (djiError == null) {
|
||||
@@ -2071,10 +2080,10 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
|
||||
int coordFrame;
|
||||
|
||||
double lat; // Lat
|
||||
double lng; // Lng
|
||||
double alt; // Alt
|
||||
double yaw;
|
||||
double lat; // Latitude
|
||||
double lng; // Longitude
|
||||
double alt; // Altitude
|
||||
double yaw; // Deg
|
||||
|
||||
double speed = 3; // m/s
|
||||
|
||||
@@ -2082,7 +2091,7 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
double vy;
|
||||
double vz;
|
||||
|
||||
double yawRate; // deg/sec
|
||||
double yawRate; // Deg/sec
|
||||
|
||||
YawDirection yawDirection = YawDirection.DEST; // Where to look while moving
|
||||
|
||||
@@ -2200,6 +2209,13 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
if(mask.ignoreVelZ) throttle = 0;
|
||||
if(mask.ignoreYawRate) yawRate = 0;
|
||||
|
||||
if(!motion.mask.ignoreYaw) {
|
||||
double currentYaw = getCurrentYaw();
|
||||
double yawError = rotation(yaw, currentYaw);
|
||||
yawRate = miniPIDHeading.getOutput(-yawError, 0);
|
||||
Log.i(TAG, "YAWRATE: yaw: " + yaw + " ; currentYaw: " + currentYaw + " ; err: " + yawError);
|
||||
}
|
||||
|
||||
setVelocities(roll, pitch, throttle, yawRate);
|
||||
}
|
||||
}
|
||||
@@ -2285,10 +2301,10 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
@Override
|
||||
public void run() {
|
||||
if (motion.isVelocityCommand) {
|
||||
motion.processVelocities(motion.coordFrame);
|
||||
|
||||
motion.velocityTicks--;
|
||||
if (motion.velocityTicks <= 0) {
|
||||
if (motion.velocityTicks > 0) {
|
||||
motion.processVelocities(motion.coordFrame);
|
||||
motion.velocityTicks--;
|
||||
} else {
|
||||
cancelMotion();
|
||||
}
|
||||
|
||||
@@ -2355,7 +2371,7 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
}
|
||||
|
||||
yawError = rotation(targetYaw, yaw);
|
||||
Log.i(TAG, "YAW - target: " + targetYaw + " ; yaw: " + yaw + " ; err: " + yawError);
|
||||
if(DEBUG_MOTION) Log.i(TAG, "YAW - target: " + targetYaw + " ; yaw: " + yaw + " ; err: " + yawError);
|
||||
}
|
||||
|
||||
// Check if we reached destination point
|
||||
@@ -2467,6 +2483,7 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
}
|
||||
|
||||
void setVirtualSticksEnabled(boolean setEnabled) {
|
||||
if(System.currentTimeMillis() < timerIgnoreMavLink) return;
|
||||
mFlightController.setVirtualStickModeEnabled(setEnabled, djiError -> {
|
||||
if (djiError != null) {
|
||||
Log.e(TAG, "setVirtualStickModeEnabled() failed (will retry): " + djiError.toString());
|
||||
@@ -2871,17 +2888,31 @@ public class DroneModel implements CommonCallbacks.CompletionCallback {
|
||||
}
|
||||
|
||||
/**
|
||||
* Shortest angle between two angles.
|
||||
* Shortest angle between two angles (including sign for direction).
|
||||
* @param alpha Angle in range [0, 360]
|
||||
* @param beta Angle in range [0, 360]
|
||||
* @return Result will be [-180, 180].
|
||||
*/
|
||||
public static double rotation(double alpha, double beta) {
|
||||
alpha = alpha % 360;
|
||||
beta = beta % 360;
|
||||
double phi = Math.abs(beta - alpha) % 360; // This is either the distance or 360 - distance
|
||||
double distance = (phi > 180) ? 360 - phi : phi;
|
||||
|
||||
if ((alpha - beta >= 0 && alpha - beta <= 180) || (alpha - beta <= -180 && alpha - beta >= -360)) {
|
||||
return distance;
|
||||
} else {
|
||||
return -distance;
|
||||
}
|
||||
}
|
||||
/*
|
||||
private double rotation(double alpha, double beta) {
|
||||
double phi = Math.abs(beta - alpha) % 360; // This is either the distance or 360 - distance
|
||||
double distance = phi > 180 ? 360 - phi : phi;
|
||||
double sign = (alpha - beta >= 0 && alpha - beta <= 180) || (alpha - beta <= -180 && alpha - beta >= -360) ? distance : distance * -1;
|
||||
return sign;
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* Calculate the intermediate position on the Earth's surface based on a starting position and a distance and direction of travel.
|
||||
|
||||
@@ -66,14 +66,18 @@ import dji.sdk.sdkmanager.DJISDKInitEvent;
|
||||
import dji.sdk.sdkmanager.DJISDKManager;
|
||||
|
||||
public class DummyProduct extends Aircraft {
|
||||
private double homeLat = 60.4094000;
|
||||
private double homeLng = 10.4910999;
|
||||
private double homeLat = -32.8540743;
|
||||
private double homeLng = -71.2153629;
|
||||
|
||||
private double lat = homeLat;
|
||||
private double lng = homeLng;
|
||||
private float alt = 30;
|
||||
private double yaw = 0;
|
||||
private boolean isFlying = true;
|
||||
|
||||
//private float alt = 30;
|
||||
//private boolean isFlying = true;
|
||||
private float alt = 0;
|
||||
private boolean isFlying = false;
|
||||
|
||||
private double yaw = 0; // Deg
|
||||
private long lastTime = 0;
|
||||
|
||||
private static final String TAG = DummyProduct.class.getSimpleName();
|
||||
@@ -578,7 +582,8 @@ public class DummyProduct extends Aircraft {
|
||||
|
||||
@Override
|
||||
public void startTakeoff(@Nullable CommonCallbacks.CompletionCallback completionCallback) {
|
||||
|
||||
isFlying = true;
|
||||
completionCallback.onResult(null);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -593,7 +598,10 @@ public class DummyProduct extends Aircraft {
|
||||
|
||||
@Override
|
||||
public void startLanding(@Nullable CommonCallbacks.CompletionCallback completionCallback) {
|
||||
|
||||
// HACK: Teletransporting to ground
|
||||
alt = 0;
|
||||
isFlying = false;
|
||||
completionCallback.onResult(null);
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -694,11 +702,17 @@ public class DummyProduct extends Aircraft {
|
||||
double rad = Math.toRadians(yaw);
|
||||
double[] dLatLng = Functions.getLatLngDiff(lat, rad, fwdVel, rightVel);
|
||||
|
||||
// Virtual stick movements are not in m/s
|
||||
// TODO: Adjust to match real drone motion
|
||||
double f1 = 0.5;
|
||||
double f2 = 0.5;
|
||||
double f3 = 0.5;
|
||||
|
||||
synchronized (this) { // Consistency
|
||||
lat += dLatLng[0] * dt;
|
||||
lng += dLatLng[1] * dt;
|
||||
yaw += yawVel * dt;
|
||||
alt += throttleVel * dt;
|
||||
lat += dLatLng[0] * dt * f1;
|
||||
lng += dLatLng[1] * dt * f1;
|
||||
alt += throttleVel * dt * f2;
|
||||
yaw += yawVel * dt * f3;
|
||||
|
||||
if (yaw > 180) {
|
||||
yaw -= 360;
|
||||
|
||||
@@ -345,7 +345,7 @@ public class MAVLinkReceiver {
|
||||
msg_set_position_target_local_ned msg_param = (msg_set_position_target_local_ned) msg;
|
||||
DroneModel.Motion motion = mModel.newMotion(MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED, msg_param.type_mask);
|
||||
motion.setDestination(msg_param.coordinate_frame, msg_param.x, msg_param.y, msg_param.z);
|
||||
motion.yaw = msg_param.yaw;
|
||||
motion.yaw = Math.toDegrees(msg_param.yaw);
|
||||
motion.yawRate = Math.toDegrees(msg_param.yaw_rate);
|
||||
motion.vx = msg_param.vx;
|
||||
motion.vy = msg_param.vy;
|
||||
|
||||
Referência em uma Nova Issue
Bloquear um usuário