Browse Source

AC_WPNav: use multiply instead of divide

non-functional change
master
Randy Mackay 7 years ago
parent
commit
5be1020578
  1. 4
      libraries/AC_WPNav/AC_WPNav.cpp

4
libraries/AC_WPNav/AC_WPNav.cpp

@ -281,7 +281,7 @@ void AC_WPNav::get_loiter_stopping_point_xy(Vector3f& stopping_point) const @@ -281,7 +281,7 @@ void AC_WPNav::get_loiter_stopping_point_xy(Vector3f& stopping_point) const
float AC_WPNav::get_loiter_angle_max_cd() const
{
if (is_zero(_loiter_angle_max)){
return MIN(_attitude_control.lean_angle_max()*2.0f/3.0f, _pos_control.get_lean_angle_max_cd()*2.0f/3.0f);
return MIN(_attitude_control.lean_angle_max(), _pos_control.get_lean_angle_max_cd()) * (2.0f/3.0f);
}
return MIN(_loiter_angle_max*100.0f, _pos_control.get_lean_angle_max_cd());
}
@ -331,7 +331,7 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit) @@ -331,7 +331,7 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
float loiter_brake_accel = 0.0f;
if (_loiter_desired_accel.is_zero()) {
if((AP_HAL::millis()-_brake_timer) > _loiter_brake_delay * 1000.0f){
float brake_gain = _pos_control.get_vel_xy_pid().kP()/2.0f;
float brake_gain = _pos_control.get_vel_xy_pid().kP() * 0.5f;
loiter_brake_accel = constrain_float(AC_AttitudeControl::sqrt_controller(desired_speed, brake_gain, _loiter_brake_jerk_max_cmsss, nav_dt), 0.0f, _loiter_brake_accel_cmss);
}
} else {

Loading…
Cancel
Save