AC_WPNav: simple avoidance using proximity sensors

rmackay9 · Aug 28, 2024 · 717c949 · 717c949
1 parent 3708349
commit 717c949
Show file tree

Hide file tree

Showing 2 changed files with 48 additions and 1 deletion.
diff --git 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 <AP_Vehicle/AP_Vehicle_Type.h>
 #include "AC_WPNav.h"
 
 extern const AP_HAL::HAL& hal;
@@ -489,10 +490,22 @@ 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 AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+        if (_simple_avoidance.vel_xy_max_set) {
+            vel_input = MIN(vel_input, _simple_avoidance.vel_xy_max);
+        }
+#endif
+
+        // 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 +531,32 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
         s_finished = _spline_this_leg.reached_destination();
     }
 
+#if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    // 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, true);
+        // check if horizontal velocity has been reduced
+        if (target_vel_adjusted.xy() != target_vel.xy()) {
+            float dir = target_vel_adjusted.xy().dot(target_vel.xy()) >= 0.0 ? 1.0 : -1.0;
+            _simple_avoidance.vel_xy_max = dir * 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;
+            }
+        }
+    }
+#endif
+
     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
@@ -290,4 +290,12 @@ 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)
+
+#if AP_AVOIDANCE_ENABLED
+    // 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;
+#endif
 };