Fix yawRate radians to degrees conversion

RosettaDrone · Dec 7, 2024 · 22d9496 · 22d9496
1 parent 4617557
commit 22d9496
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 6 deletions.
diff --git a/app/src/main/java/sq/rogue/rosettadrone/DroneModel.java b/app/src/main/java/sq/rogue/rosettadrone/DroneModel.java
@@ -2086,7 +2086,7 @@ public class Motion {
         double vy;
         double vz;
 
-        double yawRate;
+        double yawRate; // deg/sec
 
         YawDirection yawDirection = YawDirection.DEST; // Where to look while moving
 
@@ -2504,15 +2504,24 @@ void setControlModes() {
     }
 
     int velLogCounter = 0;
-    void setVelocities(double roll, double pitch, double throttle, double yaw) {
+
+    /**
+     * Set drone velocities.
+     *
+     * @param roll
+     * @param pitch
+     * @param throttle
+     * @param yawRate deg/sec
+     */
+    void setVelocities(double roll, double pitch, double throttle, double yawRate) {
         if(isForbiddenSwitchMode()) return;
 
         if(DEBUG_MOTION || velLogCounter++ >= 1000 / MOTION_PERIOD_MS) { // 1 log per second
             velLogCounter = 0;
-            Log.i(TAG, "setVelocities = fwd: " + roll + " ; right: " + pitch + " ; up: " + throttle + " ; yaw: " + yaw);
+            Log.i(TAG, "setVelocities = fwd: " + roll + " ; right: " + pitch + " ; up: " + throttle + " ; yaw: " + yawRate);
         }
 
-        mFlightController.sendVirtualStickFlightControlData(new FlightControlData((float)pitch, (float)roll, (float)yaw, (float)throttle), djiError -> {
+        mFlightController.sendVirtualStickFlightControlData(new FlightControlData((float)pitch, (float)roll, (float)yawRate, (float)throttle), djiError -> {
             if (djiError != null) Log.e(TAG, "SendVelocityDataTask Error: " + djiError.toString());
         });
     }

diff --git a/app/src/main/java/sq/rogue/rosettadrone/MAVLinkReceiver.java b/app/src/main/java/sq/rogue/rosettadrone/MAVLinkReceiver.java
@@ -346,7 +346,7 @@ public void process(MAVLinkMessage 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.yawRate = msg_param.yaw_rate;
+                motion.yawRate = Math.toDegrees(msg_param.yaw_rate);
                 motion.vx = msg_param.vx;
                 motion.vy = msg_param.vy;
                 motion.vz = msg_param.vz;
@@ -547,7 +547,7 @@ void setPositionTargetGlobal(msg_set_position_target_global_int msg) {
         } else {
             motion.setDestination(msg.coordinate_frame, msg.lat_int, msg.lon_int, msg.alt);
             motion.yaw = msg.yaw;
-            motion.yawRate = msg.yaw_rate;
+            motion.yawRate = Math.toDegrees(msg.yaw_rate);
         }
         mModel.startMotion(motion);
     }