|
|
|
@ -302,7 +302,7 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
@@ -302,7 +302,7 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
|
|
|
|
|
|
|
|
|
|
// Limit the velocity to prevent fence violations
|
|
|
|
|
if (_avoid != nullptr) { |
|
|
|
|
_avoid->adjust_velocity(_pos_control.get_pos_xy_kP(), _loiter_accel_cmss, desired_vel, nav_dt); |
|
|
|
|
_avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _loiter_accel_cmss, desired_vel, nav_dt); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// send adjusted feed forward velocity back to position controller
|
|
|
|
@ -631,7 +631,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
@@ -631,7 +631,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
|
|
|
|
|
|
|
|
|
|
// calculate point at which velocity switches from linear to sqrt
|
|
|
|
|
float linear_velocity = _wp_speed_cms; |
|
|
|
|
float kP = _pos_control.get_pos_xy_kP(); |
|
|
|
|
float kP = _pos_control.get_pos_xy_p().kP(); |
|
|
|
|
if (kP >= 0.0f) { // avoid divide by zero
|
|
|
|
|
linear_velocity = _track_accel/kP; |
|
|
|
|
} |
|
|
|
|