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

app/src/main/java/sq/rogue/rosettadrone/DroneModel.java

@@ -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 lat; // Latitude
-        double lng; // Lng
+        double lng; // Longitude
-        double alt; // Alt
+        double alt; // Altitude
-        double yaw;
+        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);
+                if (motion.velocityTicks > 0) {
-
+                    motion.processVelocities(motion.coordFrame);
-                motion.velocityTicks--;
+                    motion.velocityTicks--;
-                if (motion.velocityTicks <= 0) {
+                } 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.

app/src/main/java/sq/rogue/rosettadrone/DummyProduct.java

@@ -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 homeLat = -32.8540743;
-    private double homeLng = 10.4910999;
+    private double homeLng = -71.2153629;
 
     private double lat = homeLat;
     private double lng = homeLng;
-    private float alt = 30;
+
-    private double yaw = 0;
+    //private float alt = 30;
-    private boolean isFlying = true;
+    //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;
+                lat += dLatLng[0] * dt * f1;
-                lng += dLatLng[1] * dt;
+                lng += dLatLng[1] * dt * f1;
-                yaw += yawVel * dt;
+                alt += throttleVel * dt * f2;
-                alt += throttleVel * dt;
+                yaw += yawVel * dt * f3;
 
                 if (yaw > 180) {
                     yaw -= 360;

app/src/main/java/sq/rogue/rosettadrone/MAVLinkReceiver.java

@@ -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;