From f769a18996701b47119348bc99c4b5b9fc009458 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 3 Jan 2022 11:48:33 +0900 Subject: [PATCH] AR_WPNav: add set_desired_location_expect_fast_update --- libraries/AR_WPNav/AR_WPNav.cpp | 84 +++++++++++++++++++++++++++++++-- libraries/AR_WPNav/AR_WPNav.h | 18 ++++++- 2 files changed, 97 insertions(+), 5 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 1f374586c8..5daed036e2 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/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 ¤t_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 ¤t_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 ¤t_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; + } + } +} diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index ee4e680a7e..af112af426 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/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 ¤t_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