From ac05535d7f4e2a3e2a93d053ee226160e91e1e7d Mon Sep 17 00:00:00 2001 From: Xiaocheng Song Date: Thu, 2 Apr 2020 23:51:56 +0800 Subject: [PATCH 1/2] Call common function for waiting in rotateByYawRate Make it consistent with other API. --- AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 44d0261974..238c9acd44 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -476,12 +476,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& kp, const vector& ki, const vector& kd) From 4115e0aef59a9cb6e22a5bbf8291b9abd9808528 Mon Sep 17 00:00:00 2001 From: Xiaocheng Song Date: Fri, 3 Apr 2020 02:56:57 +0800 Subject: [PATCH 2/2] Fix bug: rotateToYaw not working - `isYawWithinMargin` should receive the target yaw degree as the first argument - the original stop criterion does not stop rotation due to inertia, so the resulted yaw is not guaranteed. --- .../multirotor/api/MultirotorApiBase.hpp | 1 + .../multirotor/api/MultirotorApiBase.cpp | 34 +++++++++++-------- 2 files changed, 20 insertions(+), 15 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index a39b831fc0..b95c22211e 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/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 diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 238c9acd44..6bd1d4c191 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/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)