From f06ff72a3aa750245c15117d90afbe9fc73b10b3 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 7 Mar 2024 16:35:16 +0900
Subject: [PATCH] AC_WPNav: implement simple avoidance

---
 libraries/AC_WPNav/AC_WPNav.cpp | 46 ++++++++++++++++++++++++++++++++-
 libraries/AC_WPNav/AC_WPNav.h   |  6 +++++
 2 files changed, 51 insertions(+), 1 deletion(-)

diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index be5c7ac97873d5..59ae86eb2a353f 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -1,4 +1,5 @@
 #include <AP_HAL/AP_HAL.h>
+#include <AC_Avoidance/AC_Avoid.h>
 #include "AC_WPNav.h"
 
 extern const AP_HAL::HAL& hal;
@@ -489,10 +490,20 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     // vel_scaler_dt scales the velocity and acceleration to be kinematically consistent
     float vel_scaler_dt = 1.0;
     if (is_positive(_wp_desired_speed_xy_cms)) {
+        // update the offset velocity using the previous iteration's acceleration
         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;
+
+        // decide on the new velocity target. zero if paused, the waypoint speed or the avoidance limited speed
+        float vel_input = !_paused ? _wp_desired_speed_xy_cms * offset_z_scaler : 0.0;
+        if (_simple_avoidance.vel_xy_max_set) {
+            vel_input = MIN(vel_input, _simple_avoidance.vel_xy_max);
+        }
+
+        // calculate the acceleration to achieve the new velocity target
         shape_vel_accel(vel_input, 0.0, _offset_vel, _offset_accel, -get_wp_acceleration(), get_wp_acceleration(),
                         _pos_control.get_shaping_jerk_xy_cmsss(), dt, true);
+
+        // calculate the time scaler which is applied later to the SCurve or Spline velocity and acceleration outputs
         vel_scaler_dt = _offset_vel / _wp_desired_speed_xy_cms;
     }
 
@@ -518,6 +529,39 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
         s_finished = _spline_this_leg.reached_destination();
     }
 
+    // use raw targets to calculate maximum avoidance velocity
+    _simple_avoidance.vel_xy_max_set = false;
+    AC_Avoid *_avoid = AP::ac_avoid();
+    if (_avoid != nullptr) {
+        Vector3f target_vel_adjusted = target_vel;
+        bool backing_up = false;
+        _avoid->adjust_velocity(target_vel_adjusted, backing_up, _pos_control.get_pos_xy_p().kP(), _pos_control.get_max_accel_xy_cmss(), _pos_control.get_pos_z_p().kP(), _pos_control.get_max_accel_z_cmss(), dt);
+        if (backing_up) {
+            // if backing up then set vel_xy_max to zero
+            _simple_avoidance.vel_xy_max = 0.0;
+            _simple_avoidance.vel_xy_max_set = true;
+        } else {
+            // check if horizontal velocity has been reduced
+            if (target_vel_adjusted.xy() != target_vel.xy()) {
+                _simple_avoidance.vel_xy_max = target_vel_adjusted.xy().projected(target_vel.xy()).length();
+                _simple_avoidance.vel_xy_max_set = true;
+            }
+            // check if vertical velocity has been reduced
+            if (!is_equal(target_vel_adjusted.z, target_vel.z) && !is_zero(target_vel.z)) {
+                // calculate a maximum horizontal velocity that will give us the desired vertical velocity
+                float vel_xy_max_equivalent = fabsf((target_vel_adjusted.z / target_vel.z) * _wp_desired_speed_xy_cms);
+                if (!_simple_avoidance.vel_xy_max_set || (vel_xy_max_equivalent < _simple_avoidance.vel_xy_max)) {
+                    _simple_avoidance.vel_xy_max = vel_xy_max_equivalent;
+                    _simple_avoidance.vel_xy_max_set = true;
+                }
+            }
+            // avoid numerical errors causing the vehicle to creep forward
+            if (_simple_avoidance.vel_xy_max_set && (_simple_avoidance.vel_xy_max < 1.0)) {
+                _simple_avoidance.vel_xy_max = 0;
+            }
+        }
+    }
+
     Vector3f accel_offset;
     if (is_positive(target_vel.length_squared())) {
         Vector3f track_direction = target_vel.normalized();
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index 5e2249597698d6..24ba51aeb380a4 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -290,4 +290,10 @@ class AC_WPNav
     AP_Int8     _rangefinder_use;       // parameter that specifies if the range finder should be used for terrain following commands
     bool        _rangefinder_healthy;   // true if rangefinder distance is healthy (i.e. between min and maximum)
     float       _rangefinder_terrain_offset_cm; // latest rangefinder based terrain offset (e.g. terrain's height above EKF origin)
+
+    // simple avoidance variables
+    struct {
+        float vel_xy_max;               // maximum horizontal velocity possible while avoiding obstacles
+        bool vel_xy_max_set;            // true if _avoid_vel_max has been set
+    } _simple_avoidance;
 };