AC_WPNav: Leonard's 3d leash calculator

libraries/AC_WPNav/AC_WPNav.cpp

@@ -109,9 +109,9 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
 
     // constrain the velocity vector and scale if necessary
     vel_total = safe_sqrt(_target_vel.x*_target_vel.x + _target_vel.y*_target_vel.y);
-    if( vel_total > MAX_LOITER_POS_VEL_VELOCITY ) {
-        _target_vel.x = MAX_LOITER_POS_VEL_VELOCITY * _target_vel.x/vel_total;
-        _target_vel.y = MAX_LOITER_POS_VEL_VELOCITY * _target_vel.y/vel_total;
+    if( vel_total > MAX_LOITER_POS_VELOCITY ) {
+        _target_vel.x = MAX_LOITER_POS_VELOCITY * _target_vel.x/vel_total;
+        _target_vel.y = MAX_LOITER_POS_VELOCITY * _target_vel.y/vel_total;
     }
 
     // update target position
@@ -157,7 +157,7 @@ void AC_WPNav::update_loiter()
 
     // translate any adjustments from pilot to loiter target
     translate_loiter_target_movements(dt);
-    
+
     // run loiter position controller
     get_loiter_pos_lat_lon(_target.x, _target.y, dt);
 }
@@ -175,25 +175,14 @@ void AC_WPNav::set_destination(const Vector3f& destination)
 /// set_origin_and_destination - set origin and destination using lat/lon coordinates
 void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
 {
-    Vector3f pos_delta = destination - origin;
     _origin = origin;
     _destination = destination;
-    _track_length = pos_delta.length();
-    _pos_delta_pct = pos_delta/_track_length;
-
-    // calculate proportion of track that is horizontal
-    if( pos_delta.x == 0 && pos_delta.y == 0 ) {
-        _hoz_track_ratio = 0;
-    }else{
-        _hoz_track_ratio = _track_length / safe_sqrt(pos_delta.x*pos_delta.x + pos_delta.y*pos_delta.y);
-    }
 
-    // calculate proportion of track that is horizontal
-    if( pos_delta.z == 0 ) {
-        _vert_track_ratio = 0;
-    }else{
-        _vert_track_ratio = _track_length / fabsf(pos_delta.z);
-    }
+    _vert_track_scale = WPINAV_MAX_POS_ERROR / WPINAV_MAX_ALT_ERROR;
+    Vector3f pos_delta = _destination - _origin;
+    pos_delta.z = pos_delta.z * _vert_track_scale;
+    _track_length = pos_delta.length();
+    _pos_delta_unit = pos_delta/_track_length;
 
     _track_desired = 0;
 }
@@ -201,50 +190,32 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
 /// advance_target_along_track - move target location along track from origin to destination
 void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
 {
-    float cross_track_dist;
+    //float cross_track_dist;
     float track_covered;
+    float track_error;
     float track_desired_max;
-    float alt_error;
-    float hor_track_allowance, vert_track_allowance;
+    float track_extra_max;
+    float curr_delta_length;
+    //float alt_error;
 
     // get current location
-    Vector3f curr = _inav->get_position();
+    Vector3f curr_delta = _inav->get_position() - _origin;
+    curr_delta.z = curr_delta.z * _vert_track_scale;
+    curr_delta_length = curr_delta.length();
 
-    // limit velocity to maximum possible
-    // to-do: assuming waypoint speed are not changed often we could move this to the set_origin_and_destination function
-    // no need to limit by horizontal speed if there is no horizontal component
-    if( _hoz_track_ratio >= 0 ) {
-        velocity_cms = min(velocity_cms, _speed_cms) * _hoz_track_ratio;
-    }
-    // no need to limit by vertical speed if there is no vertical component
-    if( _vert_track_ratio >= 0 ) {
-        velocity_cms = min(velocity_cms, _speedz_cms * _vert_track_ratio);
-    }
+    track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z;
+    track_error = safe_sqrt(curr_delta_length*curr_delta_length - track_covered*track_covered);
 
-    track_covered = (curr.x-_origin.x) * _pos_delta_pct.x + (curr.y-_origin.y) * _pos_delta_pct.y + (curr.z-_origin.z) * _pos_delta_pct.z;
-    cross_track_dist = -(curr.x-_origin.x) * _pos_delta_pct.y + (curr.y-_origin.y) * _pos_delta_pct.x;
-    alt_error = fabsf(_origin.z + _pos_delta_pct.z * track_covered - curr.z);
+    // we don't need the following but we may want to log them to see how we are going.
+    // cross_track_dist = -(curr.x-_origin.x) * _pos_delta_unit.y + (curr.y-_origin.y) * _pos_delta_unit.x;
+    // alt_error = fabsf(_origin.z + _pos_delta_unit.z * track_covered - curr.z);
 
-    // calculate maximum horizontal distance along the track that we will allow (stops target point from getting too far from the current position)
-    if( _hoz_track_ratio > 0 ) { 
-        hor_track_allowance = safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - cross_track_dist*cross_track_dist) * _hoz_track_ratio;
-        hor_track_allowance = max(hor_track_allowance, 0);
-    }
-    // calculate maximum vertical distance along the track that we will allow
-    if( _vert_track_ratio > 0 ) { 
-        vert_track_allowance = (750-alt_error) * _vert_track_ratio;
-        vert_track_allowance = max(vert_track_allowance, 0);
-    }
-    if( _hoz_track_ratio > 0 && _vert_track_ratio ) {
-        // both vertical and horizontal components so pick minimum track allowed of the two
-        track_desired_max = track_covered + min(hor_track_allowance, vert_track_allowance);
-    }else if( _hoz_track_ratio > 0 ) {
-        // only horizontal component
-        track_desired_max = track_covered + hor_track_allowance;
-    }else{
-        // only vertical component
-        track_desired_max = track_covered + vert_track_allowance;
-    }
+    track_extra_max = safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - track_error*track_error);
+
+    // we could save a sqrt by doing the following and not assigning track_error
+    // track_extra_max = safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - (curr_delta_length*curr_delta_length - track_covered*track_covered));
+
+    track_desired_max = track_covered + track_extra_max;
 
     // advance the current target
     _track_desired += velocity_cms * dt;
@@ -257,9 +228,10 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
     _track_desired = constrain(_track_desired, 0, _track_length);
 
     // recalculate the desired position
-    _target.x = _origin.x + _pos_delta_pct.x * _track_desired;
-    _target.y = _origin.y + _pos_delta_pct.y * _track_desired;
-    _target.z = _origin.z + _pos_delta_pct.z * _track_desired;   
+    _target.x = _origin.x + _pos_delta_unit.x * _track_desired;
+    _target.y = _origin.y + _pos_delta_unit.y * _track_desired;
+    _target.z = _origin.z + (_pos_delta_unit.z * _track_desired)/_vert_track_scale;
+    // we could save a divide by having a variable for 1/_vert_track_scale
 }
 
 /// get_distance_to_destination - get horizontal distance to destination in cm
@@ -421,4 +393,4 @@ void AC_WPNav::reset_I()
 
     // set last velocity to current velocity
     _vel_last = _inav->get_velocity();
-}
+}

libraries/AC_WPNav/AC_WPNav.h

@@ -18,13 +18,35 @@
 #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.
-#define WP_SPEED                        500         // default horizontal speed betwen waypoints in cm/s
+
 #define MAX_LEAN_ANGLE                  4500        // default maximum lean angle
-#define MAX_CLIMB_VELOCITY              125         // maximum climb velocity - ToDo: pull this in from main code
 
+#define MAX_LOITER_OVERSHOOT            531        // maximum distance (in cm) that we will allow the target loiter point to be from the current location when switching into loiter
+// D0 = MAX_LOITER_POS_ACCEL/(2*Pid_P^2);
+// if MAX_LOITER_POS_VELOCITY > 2*D0*Pid_P
+//     MAX_LOITER_OVERSHOOT = D0 + MAX_LOITER_POS_VELOCITY.^2 ./ (2*MAX_LOITER_POS_ACCEL);
+// else
+//     MAX_LOITER_OVERSHOOT = min(200, MAX_LOITER_POS_VELOCITY/Pid_P); // to stop it being over sensitive to error
+// end
+
+#define WP_SPEED                        500         // default horizontal speed betwen waypoints in cm/s
+#define WPINAV_MAX_POS_ERROR            531.25f     // maximum distance (in cm) that the desired track can stray from our current location.
+// D0 = MAX_LOITER_POS_ACCEL/(2*Pid_P^2);
+// if WP_SPEED > 2*D0*Pid_P
+//     WPINAV_MAX_POS_ERROR = D0 + WP_SPEED.^2 ./ (2*MAX_LOITER_POS_ACCEL);
+// else
+//     WPINAV_MAX_POS_ERROR = min(200, WP_SPEED/Pid_P); // to stop it being over sensitive to error
+// end
+// This should use the current waypoint max speed though rather than the default
+
+#define MAX_CLIMB_VELOCITY              125         // maximum climb velocity - ToDo: pull this in from main code
+#define WPINAV_MAX_ALT_ERROR            62.50f     // maximum distance (in cm) that the desired track can stray from our current location.
+// D0 = ALT_HOLD_ACCEL_MAX/(2*Pid_P^2);
+// if g.pilot_velocity_z_max > 2*D0*Pid_P
+//     WPINAV_MAX_ALT_ERROR = D0 + MAX_CLIMB_VELOCITY.^2 ./ (2*ALT_HOLD_ACCEL_MAX);
+// else
+//     WPINAV_MAX_ALT_ERROR = min(100, MAX_CLIMB_VELOCITY/Pid_P); // to stop it being over sensitive to error
+// end
 class AC_WPNav
 {
 public:
@@ -65,7 +87,7 @@ public:
 
     /// clear_angle_limit - reset angle limits back to defaults
     void clear_angle_limit() { _lean_angle_max = MAX_LEAN_ANGLE; }
-    
+
     /// get_angle_limit - retrieve maximum angle in centi-degrees the copter will lean
     int32_t get_angle_limit() { return _lean_angle_max; }
 
@@ -176,12 +198,11 @@ protected:
     Vector3f    _vel_last;              // previous iterations velocity in cm/s
     Vector3f    _origin;                // starting point of trip to next waypoint in cm from home (equivalent to next_WP)
     Vector3f    _destination;           // target destination in cm from home (equivalent to next_WP)
-    Vector3f    _pos_delta_pct;         // each axis's percentage of the total track from origin to destination
+    Vector3f    _pos_delta_unit;        // each axis's percentage of the total track from origin to destination
     float       _track_length;          // distance in cm between origin and destination
     float       _track_desired;         // our desired distance along the track in cm
     float       _distance_to_target;    // distance to loiter target
-    float       _hoz_track_ratio;       // horizontal component of track to next waypoint
-    float       _vert_track_ratio;      // vertical component of track to next waypoint
+    float       _vert_track_scale;      // vertical scaling to give altitude equal weighting to position
 
     // pilot inputs for loiter
     int16_t     _pilot_vel_forward_cms;