AR_WPNav: pivot only when destination set

this avoids unhelpful pivots as the vehicle reaches the waypoint when WP_RADIUS is very small

libraries/AR_WPNav/AR_WPNav.cpp

@@ -24,6 +24,7 @@ extern const AP_HAL::HAL& hal;
 #define AR_WPNAV_RADIUS_DEFAULT         2.0f
 #define AR_WPNAV_OVERSHOOT_DEFAULT      2.0f
 #define AR_WPNAV_PIVOT_ANGLE_DEFAULT    60
+#define AR_WPNAV_PIVOT_ANGLE_ACCURACY   10      // vehicle will pivot to within this many degrees of destination
 #define AR_WPNAV_PIVOT_RATE_DEFAULT     90
 
 const AP_Param::GroupInfo AR_WPNav::var_info[] = {
@@ -141,6 +142,11 @@ void AR_WPNav::update(float dt)
 
     update_distance_and_bearing_to_destination();
 
+    // if object avoidance is active check if vehicle should pivot towards destination
+    if (_oa_active) {
+        update_pivot_active_flag();
+    }
+
     // check if vehicle is in recovering state after switching off OA
     if (!_oa_active && _oa_was_active) {
         if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET) {
@@ -192,7 +198,10 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, float ne
     _reached_destination = false;
     update_distance_and_bearing_to_destination();
 
-    // set final desired speed
+    // determine if we should pivot immediately
+    update_pivot_active_flag();
+
+    // set final desired speed and whether vehicle should pivot
     _desired_speed_final = 0.0f;
     if (!is_equal(next_leg_bearing_cd, AR_WPNAV_HEADING_UNKNOWN)) {
         const float curr_leg_bearing_cd = _origin.get_bearing_to(_destination);
@@ -273,7 +282,7 @@ bool AR_WPNav::get_stopping_location(Location& stopping_loc)
 bool AR_WPNav::use_pivot_steering_at_next_WP(float yaw_error_cd) const
 {
     // check cases where we clearly cannot use pivot steering
-    if (!_pivot_possible || _pivot_angle <= 0) {
+    if (!_pivot_possible || _pivot_angle <= AR_WPNAV_PIVOT_ANGLE_ACCURACY) {
         return false;
     }
 
@@ -285,32 +294,32 @@ bool AR_WPNav::use_pivot_steering_at_next_WP(float yaw_error_cd) const
     return false;
 }
 
-// returns true if vehicle should pivot immediately (because heading error is too large)
-bool AR_WPNav::use_pivot_steering(float yaw_error_cd)
+// updates _pivot_active flag based on heading error to destination
+// relies on update_distance_and_bearing_to_destination having been called first
+// to update _oa_wp_bearing and _reversed variables
+void AR_WPNav::update_pivot_active_flag()
 {
     // check cases where we clearly cannot use pivot steering
-    if (!_pivot_possible || (_pivot_angle <= 0)) {
+    if (!_pivot_possible || (_pivot_angle <= AR_WPNAV_PIVOT_ANGLE_ACCURACY)) {
         _pivot_active = false;
-        return false;
+        return;
     }
 
-    // calc bearing error
-    const float yaw_error = fabsf(yaw_error_cd) * 0.01f;
+    // calc yaw error
+    const float yaw_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;
+    const float yaw_error = fabsf(wrap_180_cd(yaw_cd - AP::ahrs().yaw_sensor)) * 0.01f;
 
     // if error is larger than _pivot_angle start pivot steering
     if (yaw_error > _pivot_angle) {
         _pivot_active = true;
-        return true;
+        return;
     }
 
     // if within 10 degrees of the target heading, exit pivot steering
-    if (yaw_error < 10.0f) {
+    if (yaw_error < AR_WPNAV_PIVOT_ANGLE_ACCURACY) {
         _pivot_active = false;
-        return false;
+        return;
     }
-
-    // by default stay in
-    return _pivot_active;
 }
 
 // true if update has been called recently
@@ -350,16 +359,15 @@ void AR_WPNav::update_distance_and_bearing_to_destination()
 // relies on update_distance_and_bearing_to_destination being called first so _wp_bearing_cd has been updated
 void AR_WPNav::update_steering(const Location& current_loc, float current_speed)
 {
-    // calculate yaw error for determining if vehicle should pivot towards waypoint
-    float yaw_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;
-    float yaw_error_cd = wrap_180_cd(yaw_cd - AP::ahrs().yaw_sensor);
-
     // calculate desired turn rate and update desired heading
-    if (use_pivot_steering(yaw_error_cd)) {
+    if (_pivot_active) {
         _cross_track_error = 0.0f;
-        _desired_heading_cd = yaw_cd;
+        _desired_heading_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;;
         _desired_lat_accel = 0.0f;
-        _desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(yaw_cd * 0.01f), radians(_pivot_rate));
+        _desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(_desired_heading_cd * 0.01f), radians(_pivot_rate));
+
+        // update flag so that it can be cleared
+        update_pivot_active_flag();
     } else {
         // run L1 controller
         _nav_controller.set_reverse(_reversed);

libraries/AR_WPNav/AR_WPNav.h

@@ -104,8 +104,10 @@ private:
     // returns true if vehicle should pivot turn at next waypoint
     bool use_pivot_steering_at_next_WP(float yaw_error_cd) const;
 
-    // returns true if vehicle should pivot immediately (because heading error is too large)
-    bool use_pivot_steering(float yaw_error_cd);
+    // updates _pivot_active flag based on heading error to destination
+    // relies on update_distance_and_bearing_to_destination having been called first
+    // to update _oa_wp_bearing and _reversed variables
+    void update_pivot_active_flag();
 
     // adjust speed to ensure it does not fall below value held in SPEED_MIN
     void apply_speed_min(float &desired_speed);