去除障碍前一阶段的减速，可能会导致Loiter速度为0

libraries/AC_WPNav/AC_Loiter.cpp

@@ -106,7 +106,6 @@ void AC_Loiter::init_target(const Vector3f& position)
     _pos_control.set_desired_velocity_xy(0.0f,0.0f);
     _pos_control.set_desired_accel_xy(0.0f,0.0f);
 
-    speed_cms = _speed_cms; // 获取悬停飞行最大速度
     // initialise position controller if not already active
     if (!_pos_control.is_active_xy()) {
         _pos_control.init_xy_controller();
@@ -217,8 +216,7 @@ void AC_Loiter::update()
     }
 
     // initialise pos controller speed and acceleration
-    // _pos_control.set_max_speed_xy(_speed_cms);
+    _pos_control.set_max_speed_xy(_speed_cms);
-    _pos_control.set_max_speed_xy(speed_cms);
     _pos_control.set_max_accel_xy(_accel_cmss);
 
     calc_desired_velocity(dt);
@@ -228,8 +226,7 @@ void AC_Loiter::update()
 // sanity check parameters
 void AC_Loiter::sanity_check_params()
 {
-    // _speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN);
+    _speed_cms = MAX(_speed_cms, LOITER_SPEED_MIN);
-    speed_cms = MAX(speed_cms, LOITER_SPEED_MIN);
     _accel_cmss = MIN(_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
 }
 
@@ -242,8 +239,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt)
 
     // calculate a loiter speed limit which is the minimum of the value set by the LOITER_SPEED
     // parameter and the value set by the EKF to observe optical flow limits
-    // float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f);
+    float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f);
-    float gnd_speed_limit_cms = MIN(speed_cms, ekfGndSpdLimit*100.0f);
     gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN);
 
     float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f));

libraries/AC_WPNav/AC_Loiter.h

@@ -54,11 +54,7 @@ public:
     float get_roll() const { return _pos_control.get_roll(); }
     float get_pitch() const { return _pos_control.get_pitch(); }
 
-    float get_loiter_speed() const { return _speed_cms; }
-    void set_loiter_speed(float speed) { speed_cms = speed; }
-
     static const struct AP_Param::GroupInfo var_info[];
-
 protected:
 
     // sanity check parameters
@@ -89,6 +85,4 @@ protected:
     Vector2f    _predicted_euler_rate;
     float       _brake_timer;
     float       _brake_accel;
-    
-    float  speed_cms;
 };