AC_WPNav: improved comments

Made SPEEDZ a parameter although it's not yet used

libraries/AC_WPNav/AC_WPNav.cpp

@@ -8,11 +8,11 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
     // index 0 was used for the old orientation matrix
 
     // @Param: SPEED
-    // @DisplayName: Waypoint Speed Target
-    // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain during a WP mission
+    // @DisplayName: Waypoint Horizontal Speed Target
+    // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally during a WP mission
     // @Units: Centimeters/Second
     // @Range: 0 1000
-    // @Increment: 100
+    // @Increment: 50
     // @User: Standard
     AP_GROUPINFO("SPEED",   0, AC_WPNav, _speed_cms, WPNAV_WP_SPEED),
 
@@ -23,8 +23,18 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
     // @Range: 100 1000
     // @Increment: 1
     // @User: Standard
-    AP_GROUPINFO("RADIUS",      1, AC_WPNav, _wp_radius_cm, WPNAV_WP_RADIUS),
+    AP_GROUPINFO("RADIUS",  1, AC_WPNav, _wp_radius_cm, WPNAV_WP_RADIUS),
 
+    // @Param: SPEEDZ
+    // @DisplayName: Waypoint Vertical Speed Target
+    // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain vertically during a WP mission
+    // @Units: Centimeters/Second
+    // @Range: 0 1000
+    // @Increment: 50
+    // @User: Standard
+    AP_GROUPINFO("SPEEDZ",  2, AC_WPNav, _speedz_cms, WPNAV_WP_SPEEDZ),
+
+    
     AP_GROUPEND
 };
 
@@ -38,7 +48,6 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
     _pid_pos_lon(pid_pos_lon),
     _pid_rate_lat(pid_rate_lat),
     _pid_rate_lon(pid_rate_lon),
-    _speedz_cms(MAX_CLIMB_VELOCITY),
     _lean_angle_max(MAX_LEAN_ANGLE)
 {
     AP_Param::setup_object_defaults(this, var_info);
@@ -102,7 +111,7 @@ void AC_WPNav::move_loiter_target(float control_roll, float control_pitch, float
 /// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
 void AC_WPNav::translate_loiter_target_movements(float nav_dt)
 {
-    Vector2f target_vel_adj;    // make 2d vector?
+    Vector2f target_vel_adj;
     float vel_delta_total;
     float vel_max;
     float vel_total;
@@ -208,7 +217,7 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
     _origin = origin;
     _destination = destination;
 
-    _vert_track_scale = WPINAV_MAX_POS_ERROR / WPINAV_MAX_ALT_ERROR;
+    _vert_track_scale = WPNAV_LEASH_XY / WPNAV_LEASH_Z;
     Vector3f pos_delta = _destination - _origin;
     pos_delta.z = pos_delta.z * _vert_track_scale;
     _track_length = pos_delta.length();
@@ -237,10 +246,10 @@ void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
     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_extra_max = safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR - track_error*track_error);
+    track_extra_max = safe_sqrt(WPNAV_LEASH_XY*WPNAV_LEASH_XY - 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_extra_max = safe_sqrt(WPNAV_LEASH_XY*WPNAV_LEASH_XY - (curr_delta_length*curr_delta_length - track_covered*track_covered));
 
     track_desired_max = track_covered + track_extra_max;
 

libraries/AC_WPNav/AC_WPNav.h

@@ -29,22 +29,22 @@
 
 #define WPNAV_WP_SPEED                  500         // default horizontal speed betwen waypoints in cm/s
 #define WPNAV_WP_RADIUS                 200         // default waypoint radius in cm
-#define WPINAV_MAX_POS_ERROR            531.25f     // maximum distance (in cm) that the desired track can stray from our current location.
+#define WPNAV_LEASH_XY                  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);
+//     WPNAV_LEASH_XY = 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
+//     WPNAV_LEASH_XY = 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            100.0f      // maximum distance (in cm) that the desired track can stray from our current location.
+#define WPNAV_WP_SPEEDZ                 125         // maximum climb velocity - ToDo: pull this in from main code
+#define WPNAV_LEASH_Z                   100.0f      // 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);
+//     WPNAV_LEASH_Z = D0 + WPNAV_WP_SPEEDZ.^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
+//     WPNAV_LEASH_Z = min(100, WPNAV_WP_SPEEDZ/Pid_P); // to stop it being over sensitive to error
 // end
 class AC_WPNav
 {
@@ -183,8 +183,8 @@ protected:
     AC_PID*		_pid_rate_lon;
 
     // parameters
-    AP_Float    _speed_cms;         // default horizontal speed in cm/s
-    float       _speedz_cms;        // max vertical climb rate in cm/s.  To-Do: rename or pull this from main code
+    AP_Float    _speed_cms;         // horizontal speed target in cm/s
+    AP_Float    _speedz_cms;        // vertical speed target in cm/s
     AP_Float    _wp_radius_cm;      // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
     uint32_t	_loiter_last_update;    // time of last update_loiter call
     uint32_t	_wpnav_last_update;     // time of last update_wpnav call
@@ -196,25 +196,24 @@ protected:
     int32_t     _desired_roll;          // fed to stabilize controllers at 50hz
     int32_t     _desired_pitch;         // fed to stabilize controllers at 50hz
 
-    int32_t     _lean_angle_max;        // maximum lean angle.  can we set from main code so that throttle controller can stop leans that cause copter to lose altitude
-
-    // internal variables
+    // loiter controller internal variables
     Vector3f    _target;   		        // loiter's target location in cm from home
-    Vector3f    _target_vel;            // loiter
+    int16_t     _pilot_vel_forward_cms; // pilot's desired velocity forward (body-frame)
+    int16_t     _pilot_vel_right_cms;   // pilot's desired velocity right (body-frame)
+    Vector3f    _target_vel;            // pilot's latest desired velocity in earth-frame
     Vector3f    _vel_last;              // previous iterations velocity in cm/s
+    int32_t     _lean_angle_max;        // maximum lean angle.  can be set from main code so that throttle controller can stop leans that cause copter to lose altitude
+
+    // waypoint controller internal variables
     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_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       _vert_track_scale;      // vertical scaling to give altitude equal weighting to position
+    float       _vert_track_scale;      // vertical scaling to give altitude equal weighting to horizontal position
     bool        _reached_destination;   // true if we have reached the destination
 
-    // pilot inputs for loiter
-    int16_t     _pilot_vel_forward_cms;
-    int16_t     _pilot_vel_right_cms;
-
 public:
     // for logging purposes
     Vector2f dist_error;                // distance error calculated by loiter controller