AC_WPNav: Support pause

libraries/AC_WPNav/AC_WPNav.cpp

@@ -197,8 +197,9 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point)
     _this_leg_is_spline = false;
 
     // initialise the terrain velocity to the current maximum velocity
-    _terrain_vel = _wp_desired_speed_xy_cms;
-    _terrain_accel = 0.0;
+    _offset_vel = _wp_desired_speed_xy_cms;
+    _offset_accel = 0.0;
+    _paused = false;
 
     // mark as active
     _wp_last_update = AP_HAL::millis();
@@ -209,8 +210,8 @@ void AC_WPNav::set_speed_xy(float speed_cms)
 {
     // range check target speed and protect against divide by zero
     if (speed_cms >= WPNAV_WP_SPEED_MIN && is_positive(_wp_desired_speed_xy_cms)) {
-        // update terrain following speed scalar
-        _terrain_vel = speed_cms * _terrain_vel / _wp_desired_speed_xy_cms;
+        // update horizontal velocity speed offset scalar
+        _offset_vel = speed_cms * _offset_vel / _wp_desired_speed_xy_cms;
 
         // initialize the desired wp speed
         _wp_desired_speed_xy_cms = speed_cms;
@@ -463,14 +464,14 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
 
     // get current position and adjust altitude to origin and destination's frame (i.e. _frame)
     const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{0, 0, terr_offset};
-
-    // Use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of aircraft
     Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
     curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();
 
+    // Use _track_scalar_dt to slow down progression of the position target moving too far in front of aircraft
+    // _track_scalar_dt does not scale the velocity or acceleration
     float track_scaler_dt = 1.0f;
     // check target velocity is non-zero
-    if (is_positive(curr_target_vel.length())) {
+    if (is_positive(curr_target_vel.length_squared())) {
         Vector3f track_direction = curr_target_vel.normalized();
         const float track_error = _pos_control.get_pos_error_cm().dot(track_direction);
         const float track_velocity = _inav.get_velocity_neu_cms().dot(track_direction);
@@ -478,13 +479,15 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
         track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f);
     }
 
-    float vel_time_scalar = 1.0;
+    // Use vel_scaler_dt to slow down the trajectory time
+    // vel_scaler_dt scales the velocity and acceleration to be kinematically constent
+    float vel_scaler_dt = 1.0;
     if (is_positive(_wp_desired_speed_xy_cms)) {
-        update_vel_accel(_terrain_vel, _terrain_accel, dt, 0.0, 0.0);
-        shape_vel_accel( _wp_desired_speed_xy_cms * offset_z_scaler, 0.0,
-            _terrain_vel, _terrain_accel,
-            -_wp_accel_cmss, _wp_accel_cmss, _pos_control.get_shaping_jerk_xy_cmsss(), dt, true);
-        vel_time_scalar = _terrain_vel / _wp_desired_speed_xy_cms;
+        update_vel_accel(_offset_vel, _offset_accel, dt, 0.0, 0.0);
+        const float vel_input = !_paused ? _wp_desired_speed_xy_cms * offset_z_scaler : 0.0;
+        shape_vel_accel(vel_input, 0.0, _offset_vel, _offset_accel, -_wp_accel_cmss, _wp_accel_cmss,
+                        _pos_control.get_shaping_jerk_xy_cmsss(), dt, true);
+        vel_scaler_dt = _offset_vel / _wp_desired_speed_xy_cms;
     }
 
     // change s-curve time speed with a time constant of maximum acceleration / maximum jerk
@@ -501,16 +504,23 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     if (!_this_leg_is_spline) {
         // update target position, velocity and acceleration
         target_pos = _origin;
-        s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _scurve_accel_corner, _flags.fast_waypoint, _track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel, target_accel);
+        s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _scurve_accel_corner, _flags.fast_waypoint, _track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel, target_accel);
     } else {
         // splinetarget_vel
         target_vel = curr_target_vel;
-        _spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel);
+        _spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel);
         s_finished = _spline_this_leg.reached_destination();
     }
 
-    target_vel *= vel_time_scalar;
-    target_accel *= sq(vel_time_scalar);
+    Vector3f accel_offset;
+    if (is_positive(target_vel.length_squared())) {
+        Vector3f track_direction = target_vel.normalized();
+        accel_offset = track_direction * _offset_accel * target_vel.length() / _wp_desired_speed_xy_cms;
+    }
+
+    target_vel *= vel_scaler_dt;
+    target_accel *= sq(vel_scaler_dt);
+    target_accel += accel_offset;
 
     // convert final_target.z to altitude above the ekf origin
     target_pos.z += _pos_control.get_pos_offset_z_cm();

libraries/AC_WPNav/AC_WPNav.h

@@ -57,6 +57,13 @@ public:
     /// set current target horizontal speed during wp navigation
     void set_speed_xy(float speed_cms);
 
+    /// set pause or resume during wp navigation
+    void set_pause() { _paused = true; }
+    void set_resume() { _paused = false; }
+
+    /// get paused status
+    bool paused() { return _paused; }
+
     /// set current target climb or descent rate during wp navigation
     void set_speed_up(float speed_up_cms);
     void set_speed_down(float speed_down_cms);
@@ -258,8 +265,9 @@ protected:
     Vector3f    _origin;                // starting point of trip to next waypoint in cm from ekf origin
     Vector3f    _destination;           // target destination in cm from ekf origin
     float       _track_scalar_dt;       // time compression multiplier to slow the progress along the track
-    float       _terrain_vel;            // maximum horizontal velocity used to ensure the aircraft can maintain height above terrain
-    float       _terrain_accel;          // acceleration value used to change _terrain_vel
+    float       _offset_vel;            // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
+    float       _offset_accel;          // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
+    bool        _paused;                // flag for pausing waypoint controller
 
     // terrain following variables
     bool        _terrain_alt;   // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin