AR_WPNav: add set_desired_location_expect_fast_update

libraries/AR_WPNav/AR_WPNav.cpp

@@ -168,7 +168,14 @@ void AR_WPNav::update(float dt)
 
     // advance target along path unless vehicle is pivoting
     if (!_pivot.active()) {
-        advance_wp_target_along_track(current_loc, dt);
+        switch (_nav_control_type) {
+        case NavControllerType::NAV_SCURVE:
+            advance_wp_target_along_track(current_loc, dt);
+            break;
+        case NavControllerType::NAV_PSC_INPUT_SHAPING:
+            update_psc_input_shaping(dt);
+            break;
+        }
     }
 
     // update_steering_and_speed
@@ -179,8 +186,8 @@ void AR_WPNav::update(float dt)
 // next_destination should be provided if known to allow smooth cornering
 bool AR_WPNav::set_desired_location(const struct Location& destination, Location next_destination)
 {
-    // re-initialise if previous destination has been interrupted
-    if (!is_active() || !_reached_destination) {
+    // re-initialise if inactive, previous destination has been interrupted or different controller was used
+    if (!is_active() || !_reached_destination || (_nav_control_type != NavControllerType::NAV_SCURVE)) {
         init(0,0,0,0);
     }
 
@@ -260,6 +267,9 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
         }
     }
 
+    // scurves used for navigation to destination
+    _nav_control_type = NavControllerType::NAV_SCURVE;
+
     update_distance_and_bearing_to_destination();
 
     return true;
@@ -304,6 +314,32 @@ bool AR_WPNav::set_desired_location_NED(const Vector3f &destination, const Vecto
     return set_desired_location(dest_loc, next_dest_loc);
 }
 
+// set desired location but expect the destination to be updated again in the near future
+// position controller input shaping will be used for navigation instead of scurves
+// Note: object avoidance is not supported if this method is used
+bool AR_WPNav::set_desired_location_expect_fast_update(const Location &destination)
+{
+    // initialise if not active
+    if (!is_active() || (_nav_control_type != NavControllerType::NAV_PSC_INPUT_SHAPING)) {
+        init(0,0,0,0);
+    }
+
+    // initialise some variables
+    _origin = _destination;
+    _destination = destination;
+    _orig_and_dest_valid = true;
+    _reached_destination = false;
+
+    update_distance_and_bearing_to_destination();
+
+    // check if vehicle should pivot
+    _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01);
+
+    // position controller input shaping used for navigation to destination
+    _nav_control_type = NavControllerType::NAV_PSC_INPUT_SHAPING;
+    return true;
+}
+
 // get max acceleration in m/s/s
 float AR_WPNav::get_accel_max() const
 {
@@ -357,7 +393,7 @@ bool AR_WPNav::is_active() const
     return ((AP_HAL::millis() - _last_update_ms) < AR_WPNAV_TIMEOUT_MS);
 }
 
-// move target location along track from origin to destination
+// move target location along track from origin to destination using SCurves navigation
 void AR_WPNav::advance_wp_target_along_track(const Location &current_loc, float dt)
 {
     // exit immediately if no current location, destination or disarmed
@@ -401,6 +437,7 @@ void AR_WPNav::advance_wp_target_along_track(const Location &current_loc, float
     bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius, _fast_waypoint, _track_scalar_dt * dt, target_pos_3d_ftype, target_vel, target_accel);
 
     // pass new target to the position controller
+    init_pos_control_if_necessary();
     Vector2p target_pos_ptype{target_pos_3d_ftype.x, target_pos_3d_ftype.y};
     _pos_control.set_pos_vel_accel_target(target_pos_ptype, target_vel.xy(), target_accel.xy());
 
@@ -418,6 +455,31 @@ void AR_WPNav::advance_wp_target_along_track(const Location &current_loc, float
     }
 }
 
+// update psc input shaping navigation controller
+void AR_WPNav::update_psc_input_shaping(float dt)
+{
+    // convert destination location to offset from EKF origin (in meters)
+    Vector2f pos_target_cm;
+    if (!_destination.get_vector_xy_from_origin_NE(pos_target_cm)) {
+        return;
+    }
+
+    // initialise position controller if not called recently
+    init_pos_control_if_necessary();
+
+    // convert to meters and update target
+    const Vector2p pos_target = pos_target_cm.topostype() * 0.01;
+    _pos_control.input_pos_target(pos_target, dt);
+
+    // update reached_destination
+    if (!_reached_destination) {
+        // calculate position difference between destination and position controller input shaped target
+        Vector2p pos_target_diff = pos_target - _pos_control.get_pos_target();
+        // vehicle has reached destination when the target is within 1cm of the destination and vehicle is within waypoint radius
+        _reached_destination = (pos_target_diff.length_squared() < sq(0.01)) && (_pos_control.get_pos_error().length_squared() < sq(_radius));
+    }
+}
+
 // update distance from vehicle's current position to destination
 void AR_WPNav::update_distance_and_bearing_to_destination()
 {
@@ -516,3 +578,17 @@ float AR_WPNav::get_corner_angle(const Location& loc1, const Location& loc2, con
     const float diff_yaw_deg = wrap_180(loc2_to_loc3_deg - loc1_to_loc2_deg);
     return diff_yaw_deg;
 }
+
+// helper function to initialise position controller if it hasn't been called recently
+// this should be called before updating the position controller with new targets but after the EKF has a good position estimate
+void AR_WPNav::init_pos_control_if_necessary()
+{
+    // initialise position controller if not called recently
+    if (!_pos_control.is_active()) {
+        if (!_pos_control.init()) {
+            // this should never fail because we should always have a valid position estimate at this point
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+            return;
+        }
+    }
+}

libraries/AR_WPNav/AR_WPNav.h

@@ -54,6 +54,11 @@ public:
     bool set_desired_location_NED(const Vector3f& destination) WARN_IF_UNUSED;
     bool set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination) WARN_IF_UNUSED;
 
+    // set desired location but expect the destination to be updated again in the near future
+    // position controller input shaping will be used for navigation instead of scurves
+    // Note: object avoidance is not supported if this method is used
+    bool set_desired_location_expect_fast_update(const Location &destination) WARN_IF_UNUSED;
+
     // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
     virtual bool reached_destination() const { return _reached_destination; }
 
@@ -106,9 +111,12 @@ protected:
     // true if update has been called recently
     bool is_active() const;
 
-    // move target location along track from origin to destination
+    // move target location along track from origin to destination using SCurves navigation
     void advance_wp_target_along_track(const Location &current_loc, float dt);
 
+    // update psc input shaping navigation controller
+    void update_psc_input_shaping(float dt);
+
     // update distance and bearing from vehicle's current position to destination
     void update_distance_and_bearing_to_destination();
 
@@ -126,6 +134,10 @@ protected:
     // returns zero if the angle cannot be calculated because some points are on top of others
     float get_corner_angle(const Location& loc1, const Location& loc2, const Location& loc3) const;
 
+    // helper function to initialise position controller if it hasn't been called recently
+    // this should be called before updating the position controller with new targets but after the EKF has a good position estimate
+    void init_pos_control_if_necessary();
+
     // parameters
     AP_Float _speed_max;            // target speed between waypoints in m/s
     AP_Float _speed_min;            // target speed minimum in m/s.  Vehicle will not slow below this speed for corners
@@ -156,6 +168,10 @@ protected:
     Location _destination;          // destination Location when in Guided_WP
     bool _orig_and_dest_valid;      // true if the origin and destination have been set
     bool _reversed;                 // execute the mission by backing up
+    enum class NavControllerType {
+        NAV_SCURVE = 0,             // scurves used for navigation
+        NAV_PSC_INPUT_SHAPING       // position controller input shaping used for navigation
+    } _nav_control_type;            // navigation controller that should be used to travel from _origin to _destination
 
     // main outputs from navigation library
     float _desired_speed;           // desired speed in m/s