AC_WPNav: min accel active only when stick is centered

3drobotics · Jan 30, 2015 · d3c6695 · d3c6695
1 parent 552211f
commit d3c6695
Showing 1 changed file with 12 additions and 27 deletions.
diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -277,37 +277,22 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
     desired_vel.x += _loiter_desired_accel.x * nav_dt;
     desired_vel.y += _loiter_desired_accel.y * nav_dt;
 
-    // reduce velocity with fake wind resistance
-    if (_loiter_quadratic_drag) {
-        if (desired_vel.x > 0) {
-            desired_vel.x -= (_loiter_accel_cmss-_loiter_accel_min_cmss)*nav_dt*sq(desired_vel.x)/sq(_loiter_speed_cms);
-        } else {
-            desired_vel.x -= (_loiter_accel_cmss-_loiter_accel_min_cmss)*nav_dt*-sq(desired_vel.x)/sq(_loiter_speed_cms);
-        }
+    if (_pilot_accel_fwd_cms != 0.0f || _pilot_accel_rgt_cms != 0.0f) {
+        desired_vel.x -= (_loiter_accel_cmss)*nav_dt*desired_vel.x/_loiter_speed_cms;
+        desired_vel.y -= (_loiter_accel_cmss)*nav_dt*desired_vel.y/_loiter_speed_cms;
     } else {
         desired_vel.x -= (_loiter_accel_cmss-_loiter_accel_min_cmss)*nav_dt*desired_vel.x/_loiter_speed_cms;
-    }
-
-    if(desired_vel.x > 0 ) {
-        desired_vel.x = max(desired_vel.x - _loiter_accel_min_cmss*nav_dt, 0);
-    }else if(desired_vel.x < 0) {
-        desired_vel.x = min(desired_vel.x + _loiter_accel_min_cmss*nav_dt, 0);
-    }
-
-    if (_loiter_quadratic_drag) {
-        if (desired_vel.y > 0) {
-            desired_vel.y -= (_loiter_accel_cmss-_loiter_accel_min_cmss)*nav_dt*sq(desired_vel.y)/sq(_loiter_speed_cms);
-        } else {
-            desired_vel.y -= (_loiter_accel_cmss-_loiter_accel_min_cmss)*nav_dt*-sq(desired_vel.y)/sq(_loiter_speed_cms);
+        if(desired_vel.x > 0 ) {
+            desired_vel.x = max(desired_vel.x - _loiter_accel_min_cmss*nav_dt, 0);
+        }else if(desired_vel.x < 0) {
+            desired_vel.x = min(desired_vel.x + _loiter_accel_min_cmss*nav_dt, 0);
         }
-    } else {
         desired_vel.y -= (_loiter_accel_cmss-_loiter_accel_min_cmss)*nav_dt*desired_vel.y/_loiter_speed_cms;
-    }
-
-    if(desired_vel.y > 0 ) {
-        desired_vel.y = max(desired_vel.y - _loiter_accel_min_cmss*nav_dt, 0);
-    }else if(desired_vel.y < 0) {
-        desired_vel.y = min(desired_vel.y + _loiter_accel_min_cmss*nav_dt, 0);
+        if(desired_vel.y > 0 ) {
+            desired_vel.y = max(desired_vel.y - _loiter_accel_min_cmss*nav_dt, 0);
+        }else if(desired_vel.y < 0) {
+            desired_vel.y = min(desired_vel.y + _loiter_accel_min_cmss*nav_dt, 0);
+        }
     }
 
     // limit EKF speed limit and convert to cm/s