Fix bug: rotateToYaw not working

AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp

@@ -339,6 +339,7 @@ class MultirotorApiBase : public VehicleApiBase {
     //TODO: make this configurable?
     float landing_vel_ = 0.2f; //velocity to use for landing
     float approx_zero_vel_ = 0.05f;
+    float approx_zero_angular_vel_ = 0.01f;
 };
 
 }} //namespace

AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp

@@ -446,25 +446,29 @@ bool MultirotorApiBase::rotateToYaw(float yaw, float timeout_sec, float margin)
 {
     SingleTaskCall lock(this);
 
-    const YawMode yaw_mode(false, VectorMath::normalizeAngle(yaw));
-    Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken());
-
-    float estimated_pitch, estimated_roll, estimated_yaw;
+    if (timeout_sec <= 0)
+        return true;
 
     auto start_pos = getPosition();
-    do {
-        auto kinematics = getKinematicsEstimated();
-        VectorMath::toEulerianAngle(kinematics.pose.orientation,
-            estimated_pitch, estimated_roll, estimated_yaw);
-
-        if (isYawWithinMargin(estimated_yaw, margin))
-            return true;
+    float yaw_target = VectorMath::normalizeAngle(yaw);
+    YawMode move_yaw_mode(false, yaw_target);
+    YawMode stop_yaw_mode(true, 0);
 
-        //change yaw by moving to same position but constant yaw mode
-        moveToPositionInternal(start_pos, yaw_mode);
-    } while (waiter.sleep());
+    return waitForFunction([&]() {
+        if (isYawWithinMargin(yaw_target, margin)) { // yaw is within margin, then trying to stop rotation
+            moveToPositionInternal(start_pos, stop_yaw_mode); // let yaw rate be zero
+            auto yaw_rate = getKinematicsEstimated().twist.angular.z();
+            if (abs(yaw_rate) <= approx_zero_angular_vel_) { // already sopped
+                return true; //stop all for stably achieving the goal
+            }
+        }
+        else { // yaw is not within margin, go on rotation
+            moveToPositionInternal(start_pos, move_yaw_mode);
+        }
 
-    return false; //we are not exiting because we reached yaw
+        // yaw is not within margin
+        return false; //keep moving until timeout
+    }, timeout_sec).isComplete();
 }
 
 bool MultirotorApiBase::rotateByYawRate(float yaw_rate, float duration)
@@ -476,12 +480,11 @@ bool MultirotorApiBase::rotateByYawRate(float yaw_rate, float duration)
 
     auto start_pos = getPosition();
     YawMode yaw_mode(true, yaw_rate);
-    Waiter waiter(getCommandPeriod(), duration, getCancelToken());
-    do {
+
+    return waitForFunction([&]() {
         moveToPositionInternal(start_pos, yaw_mode);
-    } while (waiter.sleep());
-
-    return waiter.isTimeout();
+        return false; //keep moving until timeout
+    }, duration).isTimeout();
 }
 
 void MultirotorApiBase::setAngleLevelControllerGains(const vector<float>& kp, const vector<float>& ki, const vector<float>& kd)