AC_WPNav: limit max loiter position error

move interpretation of pilot input to wpnav lib

ArduCopter/ArduCopter.pde

@@ -1682,7 +1682,7 @@ void update_roll_pitch_mode(void)
         control_pitch           = g.rc_2.control_in;
 
         // update loiter target from user controls - max velocity is 5.0 m/s
-        wp_nav.move_loiter_target(-control_pitch/(2*4.5), control_roll/(2*4.5),0.01f);
+        wp_nav.move_loiter_target(control_roll, control_pitch,0.01f);
 
         // copy latest output from nav controller to stabilize controller
         nav_roll    += constrain_int32(wrap_180_cd(wp_nav.get_desired_roll()  - nav_roll),  -g.auto_slew_rate.get(), g.auto_slew_rate.get());  // 40 deg a second

libraries/AC_WPNav/AC_WPNav.cpp

@@ -71,10 +71,11 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& veloc
 }
 
 /// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
-void AC_WPNav::move_loiter_target(float vel_forward_cms, float vel_right_cms, float dt)
+void AC_WPNav::move_loiter_target(float control_roll, float control_pitch, float dt)
 {
-    _pilot_vel_forward_cms = vel_forward_cms;
-    _pilot_vel_right_cms = vel_right_cms;
+    // convert pilot input to desired velocity in cm/s
+    _pilot_vel_forward_cms = -control_pitch * MAX_LOITER_POS_VELOCITY / 4500.0f;
+    _pilot_vel_right_cms = control_roll * MAX_LOITER_POS_VELOCITY / 4500.0f;
 }
 
 /// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
@@ -116,6 +117,15 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
     // update target position
     _target.x += _target_vel.x * nav_dt;
     _target.y += _target_vel.y * nav_dt;
+
+    // constrain target position to within reasonable distance of current location
+    Vector3f curr_pos = _inav->get_position();
+    Vector3f distance_err = _target - curr_pos;
+    float distance = safe_sqrt(distance_err.x*distance_err.x + distance_err.y*distance_err.y);
+    if( distance > MAX_LOITER_OVERSHOOT ) {
+        _target.x = curr_pos.x + MAX_LOITER_OVERSHOOT * distance_err.x/distance;
+        _target.y = curr_pos.y + MAX_LOITER_OVERSHOOT * distance_err.y/distance;
+    }
 }
 
 /// get_distance_to_target - get horizontal distance to loiter target in cm

libraries/AC_WPNav/AC_WPNav.h

@@ -13,9 +13,11 @@
 #include <AP_InertialNav.h>     // Inertial Navigation library
 
 // loiter maximum velocities and accelerations
-#define MAX_LOITER_POS_VELOCITY         750         // should be 1.5 ~ 2.0 times the pilot input's max velocity
-#define MAX_LOITER_POS_ACCEL            250         // maximum acceleration in cm/s
-#define MAX_LOITER_VEL_ACCEL            400         // should be 1.5 times larger than MAX_LOITER_POS_ACCEL
+#define MAX_LOITER_POS_VELOCITY         500         // maximum velocity that our position controller will request.  should be 1.5 ~ 2.0 times the pilot input's max velocity.  To-Do: make consistent with maximum velocity requested by pilot input to loiter
+#define MAX_LOITER_POS_ACCEL            250         // defines the velocity vs distant curve.  maximum acceleration in cm/s/s that loiter position controller asks for from acceleration controller
+#define MAX_LOITER_VEL_ACCEL            400         // max acceleration in cm/s that the loiter velocity controller will ask from the lower accel controller.
+                                                    // should be 1.5 times larger than MAX_LOITER_POS_ACCEL.
+                                                    // max acceleration = max lean angle * 980 * pi / 180.  i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
 #define MAX_LOITER_POS_VEL_VELOCITY     1000
 #define MAX_LOITER_OVERSHOOT            1000        // maximum distance (in cm) that we will allow the target loiter point to be from the current location when switching into loiter
 #define WPINAV_MAX_POS_ERROR            2000.0f     // maximum distance (in cm) that the desired track can stray from our current location.
@@ -43,8 +45,8 @@ public:
     /// set_loiter_target - set initial loiter target based on current position and velocity
     void set_loiter_target(const Vector3f& position, const Vector3f& velocity);
 
-    /// move_loiter_target - move destination using forward and right velocities in cm/s
-    void move_loiter_target(float vel_forward_cms, float vel_right_cms, float dt);
+    /// move_loiter_target - move destination using pilot input
+    void move_loiter_target(float control_roll, float control_pitch, float dt);
 
     /// get_distance_to_target - get horizontal distance to loiter target in cm
     float get_distance_to_target();