From c387659374b51cf989fd1ae10fdd66c3860a39a0 Mon Sep 17 00:00:00 2001
From: xianglunkai <1322099375@qq.com>
Date: Mon, 8 Aug 2022 14:28:21 +0800
Subject: [PATCH] AP_OABendyRuler: correct margin consider vehicle speed

---
 libraries/AC_Avoidance/AP_OABendyRuler.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp
index f6387a6b3a..35acc03de3 100644
--- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp
+++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp
@@ -114,6 +114,9 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
         ground_course_deg = degrees(ground_speed_vec.angle());
     }
 
+    // consider self driving speed, planning frequency is 1Hz default
+    _margin_max += ground_speed_vec.length() * 1.0f;
+
     bool ret;
     switch (get_type()) {
         case OABendyType::OA_BENDY_VERTICAL: